t 


PROGRAM  FOR  THE  SIMULTANEOUS  ESTIMATION 
OF  DISPLACEMENT  AND  ORIENTATION  CORREC¬ 
TIONS  FOR  SEVERAL  SHORT  BASE  LINE  ARRAYS. 

by 

Robert  R.  Read 


QA 

275 

tU2 

1985 


71  ro 


PROGRAM  FOR  THE  SIMULTANEOUS  ESTIMATION 
OF  DISPLACEMENT  AND  ORIENTATION  CORREC¬ 
TIONS  FOR  SEVERAL  SHORT  BASE  LINE  ARRAYS. 

by 

*  Robert  R.  Read 


QA 

75 

42 

1985 


NPS55-85-028 

NAVAL  POSTGRADUATE  SCHOOL 

Monterey,  California 


TECHNICAL 


PROGRAM  FOR  THE  SIMULTANEOUS  ESTIMATION 
OF  DISPLACEMENT  AND  ORIENTATION 
CORRECTIONS  FOR  SEVERAL  SHORT 
BASE  LINE  ARRAYS 


ROBERT  R.  READ 

t  / 


NOVEMBER  1985 


Approved  for  public  release;  distribution  unlimited. 
Prepared  for: 

Naval  Undersea  Warfare  Engineering  Station 
Keyport,  WA  98345 


20091105012 


5l1£ 

’Ryz  naval  postgraduate  school 

Monterey,  California 

D.  A.  Schrady 
Provost 


Rear  Admiral  R.  H.  Shumaker 
Superintendent 


Reproduction  of  all  or  part  of  this  report  is  authorized. 
This  report  was  prepared  by: 


■  <32.  <$~ 

ROBERT  R.  READ 
Professor  of 
Operations  Research 


Reviewed  by: 


Released  by: 


£  ^  L- 

WASHBURN 


ALAN  R. 

Chairman 

Department  of  Operations  Research 


uir.  M 


KNEALE  T.  MARSHALL 
Dean  of  Information  anc 
Policy  Sciences 


UNCLASSIFIED 

security  Classification  o-  this  page 


REPORT  DOCUMENTATION  PAGE 


a  Rt  ORT  SECURITY  CLASSIFICATION 

UNCLASSIFIED 


lb.  RESTRICTIVE  MARKINGS 


2a.  SECURITY  CLASSIFICATION  AUTHORITY 


2b.  DECLASSIFICATION /DOWNGRADING  SCHEDULE 


i  DISTRIBUTION  /  AVAILABILITY  OF  REPORT 

Approved  for  public  release;  distribution 
unlimited. 


4  PERFORMING  ORGANIZATION  REPORT  NUMBER(S) 

NPS55-85-028 


S  MONITORING  ORGANIZATION  REPORT  NUMBER(S) 


6a  NAME  OF  PERFORMING  ORGANIZATION 

Naval  Postgraduate  School 


60  OFFICE  SYMBOL 
(If  applicable) 


7a.  NAME  OF  MONITORING  ORGANIZATION 

Naval  Undersea  Warfare  Engineering  Station 


6c  ADDRESS  (Gry,  Srare,  and  ZIP  Code) 

Monterey,  CA  93943-5100 


7b.  ADDRESS  (C/ry,  State,  and  ZIP  Code) 

Keyport,  WA  98345 


8a  NAME  OF  FUNDING /SPONSORING 
ORGANIZATION 


80  OFFICE  SYMBOL 
(If  applicable) 


9  PROCUREMENT  INSTRUMENT  IDENTIFICATION  NUMBER 


Be.  ADDRESS  (City,  State,  and  ZIP  Code) 


10  SOURCE  OF  FUNDING  NUMBERS 


program 

project 

task., 

WORK  UNIT 

element  no 

NO 

no 

ACCESSION  NO 

ii  TITLE  (include  Security  Classification) 

PROGRAM  FOR  THE  SIMULTANEOUS  ESTIMATION  OF  DISPLACEMENT  AND  ORIENTATION  CORRECTIONS  FOR 
SEVERAL  SHORT  BASE  LINE  ARRAYS 


12  PERSONAL  AUTHOR(S) 

Read,  Robert  R. 


13a  TYPE  OF  REPORT 

13b  TIME  COVERED 

14  DATE  OF  REPORT  (Year  Month,  Day) 

IS  PAGE  COUNT 

Technical 

FROM  TO 

1985  Novemhpr 

70 

'6  SUPPLEMENTARY  NOTATION 


17  COSATI  CODES 

FIELD 

GROUP 

SUB-GROUP 

18  SU8JECT  TERMS  (Continue  on  reverse 

Least  Squares,  Calibration, 
Multivariate  Regression 


\t  necessary  and  identify  by  block  numoer) 

Multivariate  Estimation, 


'9 


ABSTRACT  ( Continue  on  reverse  if  necessary  and  identify  by  block  numoer) 

This  report  presents  the  mathematical  support  and  algorithms  for  the  least  square 
estimation  of  multiple  array  displacement  and  orientation  corrections.  Two  situations  are 
treated:  (i)  cross  over  data  collected  in  the  array  overlap  region;  and  (ii)  matching  of 
first  principal  components  straight  lines  for  use  when  there  is  no  replication  of  data  in 
the  array  overlap  region.  Fortran  source  codes  are  provided  for  the  first  situation. 


20  Distribution /availability  of  abstract 
C  unclassified/unlimited  □  same  as  rpt  □  dt'C  users 


21  abstract  security  classification 


22a  NAME  OF  RESPONSIBLE  INDIVIDUAL 

Robert  R.  Read 


22b  TELEPHONE  (Include  Area  Code) 

(408)646-2382 


22c  OFFICE  SYMBOL 

Code  55Re 


DO  FORM  1473,  B4  mar 


83  APR  edition  may  be  used  until  exnausted 
All  other  editions  are  obsolete 


security  classification  of  this  page 


I. 


INTRODUCTION 


The  methodology  presented  here  is  concerned  with  the  cali¬ 
bration  (more  precisely,  the  maintenance  of  calibration)  of  a 
three-dimensional  tracking  system.  The  individual  sensor  arrays 
are  the  short  base  line  arrays  that  produce  3-D  track  for  NUWES . 

Our  task  is  to  consider  the  coherence  of  track  produced  by  two 
arrays  in  the  regions  of  array  overlap.  These  regions  make  the 
continuous  tracking  of  a  target  achievable.  Thus  many  arrays  may 
contribute  to  the  production  of  track  for  a  single  target  and  the 
integration  of  the  various  track  segments  provided  by  the  indi¬ 
vidual  arrays  is  the  main  problem  in  the  maintenance  of  calibration. 

An  individual  track  segment  produced  by  a  single  array  is 
described  originally  in  the  local  coordinate  system  of  that  array. 
Such  a  segment  must  be  transformed  to  the  Range  coordinate  system 
and  integrated  with  other  transformed  segments  to  form  a  path  for 
the  target.  These  transformations  are  assumed  to  be  linear  and 
require  two  kinds  of  input;  (i)  the  location  of  each  array  in  the 
Range  coordinate  system;  and  (ii)  the  orientation  of  the  local 
coordinate  system  relative  to  the  Range  orientation.  In  underwater 
tracking  the  location  and  orientation  of  a  local  coordinate  sys¬ 
tem  must  be  determined  remotely.  It  is  inferred  from  synchronously 
timed  track  segments. 

Each  array  has  four  sonar  receivers  located  at  four  of  the 
corners  of  a  rigid  cube.  The  target  is  a  torpedo  (or  other 
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self-propelled  vehicle) .  It  has  a  "pinger"  attached  to  it  which 
emits  a  sound  wave  at  regularly  timed  intervals  called  point  counts. 
The  position  of  the  target  at  a  given  point  count  is  determined 
from  the  sound  wave's  time  of  arrival  differential  at  the  four 
corners  containing  the  receivers.  These  signals  are  transmitted 
over  cables  to  a  central  computer. 

The  functioning  of  the  Range's  tracking  system  requires  knowledge 
of  the  location  and  orientation  of  each  array.  Moreover,  once  these 
values  are  established,  they  must  be  monitored  regularly  to  guard 
against  slippages  of  various  forms,  i.e.,  calibration  must  be 
maintained. 

There  are  a  number  of  instrumentation,  measurement  and  environ¬ 
mental  problems  associated  with  this  type  of  system,  but  they  are 
of  no  concern  in  the  present  work.  Our  goal  is  restricted  to 
estimating  array  positions  and  orientations.  Mathematically  the 
former  is  a  three-dimensional  vector  and  the  latter  is  a  three  by 
three  coefficient  matrix  that  is  constrained  to  be  orthonormal, 
that  is,  a  rigid  rotation  of  an  array's  cube  in  three  space. 

This  model  for  describing  the  changes  in  processing  the  local 
track  from  an  array  would  be  exact  if  the  speed  of  sound  in  water  were 
constant.  Such  is  not  quite  the  case.  This  speed  increases  with 
depth  and  is  assumed  to  be  homogeneous  in  each  horizontal  layer  of 
water.  Because  of  the  depth  gradient  there  is  a  non-linear  correc¬ 
tion  term  in  the  vertical  position  of  the  object.  Generally  it 
is  not  very  large.  If  an  array  had  sunk  fifty  feet  deeper  in 
a  muddy  bottom  and  a  typical  speed  with  depth  gradient  were  used 
then  sound  ray  tracing  computations  show  that  the  change  in 
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position  of  a  target  some  3000  feet  away  is  within  a  foot  of  that 
computed  using  the  linear  model.  For  NUWES  purposes  it  is  safe 
to  use  the  linear  model  to  detect  slippage.  If  the  non-linear 
term  were  suspected  of  being  important  then  the  estimation 
methodology  could  be  iterated.  That  is,  the  local  coordinate 
system  could  be  corrected  using  the  linear  methodology  followed 
by  a  recomputation  of  the  target's  track.  Then  a  reexamination 
of  the  consequences  of  this  could  be  used  to  decide  whether  the 
local  coordinate  system  should  be  corrected  again. 

It  will  be  seen  that  the  corrections  estimated  for  an  array  are 
not  absolute  but  relative  to  the  other  arrays  in  the  system.  Be¬ 
cause  of  this  it  is  convenient  to  assume  that  the  location  and 
orientation  of  at  least  one  array,  relative  to  the  Range,  were  al¬ 
ready  established.  This  fact  has  implications  in  the  calibration 
maintenance  problem.  That  is,  the  user  places  faith  in  the  current 
official  positioning  of  one  or  more  arrays.  Then  possible  changes 
in  the  positioning  of  others  are  estimated.  If  the  user  chooses 
to  change  his  selection  of  this  "anchor"  array,  the  estimated 
changes  for  the  others  must  be  adjusted  mathematically. 

The  regions  of  overlap  for  two  or  more  arrays  will  be  called 
crossover  regions.  Crossover  data  refers  to  the  tracks  (in  Range 
coordinates)  in  the  crossover  region  which  are  produced  by  two  or  more 
arrays  for  a  common  set  of  point  counts  (i.e.,  time  points).  The 
estimation  of  array  positioning  changes  is  performed  by  attempting  to 
make  crossover  data  agree  as  much  as  possible  in  some  global  sense. 

We  adopt  the  principle  of  least  squares  to  serve  in  this  role. 
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Experience  with  the  least  squares  approach  has  revealed  some 
occasional  shortcomings.  There  exist  cases  for  which  the  least 
square  objective  function  is  a  rather  flat  surface  as  a  function 
of  the  parameters  that  describe  the  changes.  (This  has  occurred 
only  when  the  number  of  arrays  in  a  problem  is  two.)  Since  this 
can  result  in  rather  large  estimated  changes  a  second  method, 
called  the  principal  components  method,  has  been  developed.  It 
involves  the  fitting  of  straight  lines  to  the  track  segments  in 
the  crossover  region,  and  this  chosen  array  positions  changes 
that  will  bring  these  lines  together.  It  has  worked  well  for  the 
cases  that  cause  trouble  in  the  least  squares  approach. 

A  brief  comparison  of  the  two  methods  follows:  The  principal 
components  method  does  not  require  crossover  data,  i.e.,  two  or 
more  versions  of  track  for  a  common  set  of  point  counts.  It  does 
require  that  the  two  segments  of  track  being  matched  should  be 
straight.  Also,  if  the  data  are  not  literally  of  the  crossover 
type,  then  one  must  also  estimate  the  distance  between  the  segments 
being  aligned.  The  least  squares  approach  requires  crossover 
data,  but  allows  the  target  to  be  maneuvering  in  the  crossover  region. 

The  paper  is  organized  as  follows:  The  second  section  con¬ 
tains  the  development  of  the  least  squares  method  for  the  simplified 
case  of  two  arrays  producing  track  in  a  single  crossover  region. 

The  main  features  of  the  method  can  be  presented  in  this  setting. 

In  the  third  section  the  application  of  these  ideas  is  extended 
to  the  general  case  of  multiple  arrays  and  a  multiple  set  of  cross¬ 
over  regions.  After  that  we  develop  the  principal  component 
method--again  treating  the  two  array  problem  first. 
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A  Fortran  77  program,  KEYMAIN,  has  been  developed  to  produce 
the  correction  estimates  from  crossover  data.  The  source  codes 
for  this  program  and  all  subroutines  are  contained  in  the  Appendix. 
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II.  MATHEMATICAL  PRELIMINARIES 


In  the  formulas  that  follow  z  will  always  be  a  scalar,  A  and 
V  column  vectors,  and  B  a  square  matrix.  The  quantities  3z/3C 
will  have  the  same  algebraic  structure  as  C  and  the  elements 
will  be  the  partial  derivatives  of  z  with  respect  to  the  elements 
of  C.  The  superscript  T  refers  to  matrix  transpose.  The 
following  formulas  are  recorded: 


If 

z 

T 

=  V  A 

then 

3z/3A  =  V 

(2.1) 

If 

z 

T 

=  A  A 

then 

3z/3A  =  2A 

(2.2) 

If 

z 

T 

=  V  BA 

then 

3z/3B  =  VAT 

(2.3) 

If 

z 

T  T 

=  V  B  V 

then 

3z/3B  =  2BWT 

(2.4) 

Also  the  trace  of  a  matrix  (sum  of  the  diagonal  elements)  will 
play  a  major  role.  If  B  and  C  are  square  matrices  of  the  same 
order,  liberal  use  will  be  made  of  the  facts  that 

Trace (BC)  =  Trace (CB)  (2.5) 

and  that  the  trace  is  a  linear  operator.  The  formulas  (2.1)  to 
(2.5)  can  be  found  in  Reference  3,  p.  7ff,  and  they  are  not 
difficult  to  develop  directly. 

Consider  a  set  of  point  counts  S  in  a  crossover  region,  and 
let  the  crossover  data  (in  Range  coordinates)  be 


6 


y(t) 


for  t  e  S 


AJ 

H 

>i 

(  xi(tM 

y(t)  = 

r2(t> 

and  x  ( t)  =  ^  X2  (t)  > 

(  y3(t)  ) 

(  x3 (t)  ) 

provided  by  two  different  sensing  arrays.  Let  us  agree  that  the 

y(t)  data  comes  from  the  array  whose  location  and  orientation 

are  established  and  our  goal  is  to  check  the  calibration  of  the 

other  array.  In  particular,  does  there  exist  a  3  vector  A  ^  0,  and  a 

3x3  matrix  B  (^  I,  the  identity  matrix)  such  that  the  adjusted  track 
values , 

/v 

x  (t)  =  A  +  B  x (t)  ,  (2.6) 

are  in  better  agreement  with  the  y(t)  than  are  the  unmodified  x(t). 

The  vector  A  is  related  to  a  displacement  of  the  sensing 
array  and  the  matrix  B  is  related  to  a  correction  of  its  orien¬ 
tation.  If  we  let  E,  (t)  be  x(t)  in  the  local  coordinate  system,  a  be 
the  location  (in  Range  coordinates)  of  the  array,  and  0  the 
(orthonormal)  orientation  adjustment  to  the  local  coordinates  then 

x (t)  =  a  +  0£ (t) 


and 


x  ( t )  =  A  +  Ba  +  B0£(t)  (2.7) 

The  corrected  location  and  orientation  adjustments  are  A+Ba  and 
B0 ,  respectively. 
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Our  immediate  goal  is  to  estimate  A  and  B  using  the  principle 

of  least  squares.  We  will  minimize  the  average  square  deviation 

between  y(t)  and  A+Bx(t)  for  each  point  count.  Using  the  squared 

2  T 

norm  notation,  ||v||  =  V  V, 

Q  =  Ave  ||  y  ( t)  -A  -  Bx  ( t)  ||  2  (2.8) 

t 

and,  since  what  follows  involves  a  modification  of  the  standard 
multivariate  regression  development,  let  us  review  an  outline  of 
the  details. 

First,  the  estimator  for  A  is 

A  ✓S 

A  =  y  -  B  x  (2.9) 

where  y  =  Ave  y(t)  and  x  =  Ave  x(t) .  The  proof  follows  from  an 
t  t 

expansion  of  (2.8),  namely 

Q  =  Ave[y(t)  -  Bx(t)]T[y(t)  -Bx(t)] 
t  ~ 

-  2  Ave[y(t)  -Bx(t)]TA  +  ATA 
t  " 

to  which  we  apply  the  rules  (2.1)  and  (2.2) , 

=  -2  Ave (y ( t)  -  Bx ( t )  -  A]  . 

Set  this  equal  to  zero,  solve  for  A  and  the  result  follows. 
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Next,  let  us  make  a  change  and  work  with  the  deviations 


Y(t)  =  y(t)  -  y  and  X(t)  =  x(t)  -  x 


(2.10) 


and  use  (2.9)  in  (2.8).  The  result  is  a  simplified  appearance 
for  Q, 


Q  =  Ave  ||  Y  (t)  -  BX  ( t ) 
t 


(2.11) 


Also  introduce  the  covariance  matrices 


yy 


Ave  {Y  (t)  Y  (t)  } 
t 


yx 


Ave{Y(t)X  (t)  } 
t 


XX 


Ave  (X(t)X  (t)  } 
t 


(2.12) 


Now  the  customary  estimator  for  B  can  be  derived  readily.  Thus 


Q  =  Ave{YT  (t)  Y  (t)  -2y'1  (t)BX(t)  +  XT  ( t)  BTBX  ( t )  } 


(2.13) 


and  use  (2.3)  and  (2.4)  to  get 


=  -2  Ave{Y (t) XT (t)  -  BX (t ) XT (t) } 


=  -2  { D  -  Bd 

yx  xx 


(2.14) 


*  -l 

using  (2.12) .  Set  this  equal  to  zero  and  solve  for  B  =  D  D 

^  ^  yx  xx 

which  is  the  usual  multivariate  regression  estimator,  Ref.  1,  p.  181. 
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Generally,  the  above  usual  estimator  is  not  the  one  we  should 
use.  Recall  that  the  sensing  arrays  are  rigid  cubes.  If  they 
have  slipped  (i.e.,  moved  physically  such  as  by  the  action  of  a 
ship's  anchor  hooking  a  cable)  then  they  undergo  a  displacement 
and  a  re-orientation.  The  matrix  B  should  be  orthonormal  as  is 
the  matrix  3  in  the  original  calibration.  Thus 

BTB  =  I  (2.15) 

and  the  estimation  of  B  involves  the  minimization  of  Q  subject  to 
the  constraint.  The  usual  estimate  would  be  useful  only  if  there 
were  physical  damage  to  an  array,  resulting  in  its  behavior  as  a 
skewed  rather  than  Cartesion  system. 

Use  of  (2.15)  and  (2.12)  in  (2.13)  allows  the  rewrite 

Q  =  Trace {Dyy  -  2DyxBT  +  DxX>  (2.16) 

and  the  minimization  of  Q  is  tantamount  to  the  maximization  of 

W  =  Trace{DyX  BT}  subject  to  (2.15)  (2.17) 

since  the  trace  is  a  linear  operator  and  B  is  involved  only  in 
the  second  term  of  (2.16).  The  solution  of  the  optimization 
problem  (2.17)  and  its  generalization  in  Section  4  is  the  bulk 
of  our  effort. 

Earlier  was  stated  the  need  for  placing  faith  in  the  location 
and  orientation  of  at  least  one  array,  because  otherwise  the 
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solution  could  not  be  unique.  Let  us  show  now,  using  analysis, 
why  this  is  the  case  in  the  two  array,  single  crossover  region 
problem.  Suppose  we  tried  to  adjust  both  data  sets,  i.e.. 


y(t)  =  +  B^y(t)  and 

/\ 

x(t)  =  A2  +  B2x(t) 


(2.18) 


when  both  and  B2  are  constrained  to  be  orthonormal.  The  least 
squares  objective  function  has  the  appearance 


Q 


Ave 

t 


^l+Biy(t)  -  a2  -B2x(t) 


(2.19) 


It  is  clear  from  the  development  of  equation  (2.9)  that  the  dis¬ 
placement  portion  of  the  optimization  can  be  estimated  only 
relatively.  That  is,  only  A  =  A2  -  A^  can  be  found  uniquely. 

A  A  A 

From  the  earlier  development  we  can  infer  that  A  =  B^y  -  B2x 
and  re-express  the  objective  as 

Q(B1,B2)  =  Ave  ||B1Y(t) -B2X(t)  ||  2  (2.20) 


But  this  is  the  squared  lengths  of  a  set  of  vectors  which  have 
been  averaged.  Since  the  squared  length  of  any  vector  is  invari¬ 
ant  under  any  orthonormal  transformation,  say  C,  it  follows  that 


Q(B1,B2)  =  QfCB^CB^  . 


In  particular  we  can  take 
then  Q(B1,B2)  =  Q(i,bJ) . 


T 

C  =  B^  (i.e.,  the  inverse  of  B^)  and 
The  product  of  orthonormal  matrices 
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T 

is  itself  orthonormal.  We  can  set  B  =  B^^  an<^  note  that  the 
minimization  of  Q(I,B)  is  that  of  (2.11),  the  problem  we  are 
treating.  The  geometrical  interpretation  is  that  the  orientation 
of  a  sensing  array  can  be  matched  only  relatively  to  the  other 
array.  It  is  convenient  to  assume  that  the  latter  orientation 
is  known. 
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III.  SOLUTION  ALGORITHM 


The  matrix  B  has  nine  elements,  but  because  of  the  constraint 
(2.15)  there  are  only  three  degrees  of  freedom.  That  is,  there 
are  three  length  constraints  and  three  orthogonality  constraints. 
The  three  remaining  degrees  of  freedom  can  be  expressed  in  terms 
of  the  Euler  angles.  Ref.  2,  p.  250  ff. 

Any  orthnormal  transformation  in  3-space  may  be  viewed  as  the 
application  of  three  successive  rotations,  i.e.,  hold  the  x^-axis 
fixed  and  execute  a  rotation  in  the  x^-x^  plane;  then  hold  the 
X2~axis  fixed  in  its  new  position  and  rotate  in  the  x^-x^  plane; 
and  finally  rotate  in  the  x^-x2  plan®  with  the  x^-axis  held  fixed 
in  its  new  position.  Denote  the  angles  of  these  three  rotations 
as  r  <pj  •  In  navigation  work  they  are  called  roll,  pitch 

and  yaw.  These  angles  are  unique  only  when  the  order  of  applica¬ 
tion  of  the  rotations  is  specified. 

To  shorten  the  writing,  let 


c^  =  cos(^)  and  s^  =  sin(<|K)  (3.1) 

for  i  =  1,2,3,  and  the  individual  planar  rotations  can  be  expressed 
as 


(3.2) 
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and,  using  the  order  of  application  described,  let 


B 


(3.3) 


We  make  liberal  use  of  the  fact  that  the  product  of  orthonormal 
matrices  is  itself  orthonormal. 

We  note  in  passing  that  the  matrices  above  are  the  trans¬ 
pose  (and  hence  inverses)  of  those  that  are  usually  used  in 
studying  the  motion  of  aeroplanes.  Ref.  [2].  Also  the  order  of 
application  is  the  reverse  of  the  customary  one  thus  making  B  in 
(3.3)  the  inverse  of  the  usual  matrix.  This  choice  is  appropriate 
for  our  work  since  the  angular  correction  will  rotate  the  local 
axes  back  to  where  they  are  expected  to  be. 

This  done  our  optimization  problem  may  be  stated  as 


Maximize  Trace {D, 

<J>l,<t)2,<{,3 


(3.4) 


yx 


The  solution  can  be  found  by  treating  the  problem  as  three 
successive  two-dimensional  problems.  To  do  this  let  us  first 
characterize  how  this  optimization  problem  would  be  solved  if 
the  tracking  took  place  in  two  dimensions. 

In  the  plane,  an  orthonormal  matrix  B  has  one  degree  of 
freedom  and  it  is  characterized  by  a  rotation  through  an  angle  <J> . 
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As  before,  let  c  =  cos(<f>)  ,  s  =  sin(<{))  so  that 


B  = 


c  -s 


s  c 


and 

W  =  Trace  (DBT)  =  (Du+D22)c+  (D2i-D12(s)  (3.5) 

Thus  W  is  itself  a  sine  wave.  It  has  one  max  and  one  min,  and  these  are 
separated  by  ir  radians.  Since 

W'  =  -(D.^  +d22)s  +  ^D21  -D12^C  and 


W  =  -(DU  +D22)o  -  (D21-D12)s 


it  is  seen  easily  that  the  max  occurs  when 


s  =  K(D2l  -D^)  '  c  =  K  (D]_i  +D22^  '  and 


K  ^  (D21  °12  ^  +  ^Dll  +  D22  ^  * 


-1/2 


(3.6) 


This  would  be  the  solution  if  the  tracking  problem  were  a  two- 
dimensional  one. 

Now  we  are  positioned  to  treat  our  three-dimensional  tracking 
problem.  Introduce  matrices 


E  = 


T  T 
D  p i  p_ 
yxKlK2 


F  = 


T  T 

P->D  p. 
K3  yxKl 


G  = 


T  T^ 

PtP,D 

2  3  yx 


(3.7) 
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and  note  that,  using  (2.5),  (2.17)  and  (3.3), 


T  T 

W  =  Trace(D  B  }  =  Trace(Ep  -J 

yx  j 

=  TracetFp^}  =  TracetGp^}  (3.8) 

These  latter  three  representatives  of  W  allow  us  to  use  our 
knowledge  of  the  solution  to  the  two-dimensional  problem.  When 
these  three  traces  are  written  out  in  expanded  form  and  compared 
with  (3.5)  and  (3.6),  it  is  seen  that  the  optimal  solution  for 
4>^,  4>2 ,  $3  must  have  the  form 

s3  =  K3^E21-E12^  and  c3  =  K3^E11+E22^ 

s2  =  K2^F31~F13^  and  c2  =  K2^Fll+F33^  (3.9) 

S1  =  K1^G32-G23^  and  C1  =  K1^G22+G33^ 


where 


K3  ^E21  E12  ^  +  ^Ell  +E22^  ^ 


-1/2 


K2  ^F3l“Fl3^  +  ^Fll+F33^  ^ 


-1/2 


(3.10) 


K1  ^G32  G23^  +  ^G2 2  +  G  33  ^  ^ 


-1/2 
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The  above  does  not  provide  explicit  solutions  because 
E  =  E  (<f>i ,  <f>2 )  /  F  =  F  ( <J>^  /  <J>2 )  and  G  =  G(<J>2  r  $3)  •  It  is  necessary  to 
get  all  three  angles  satisfying  (3.9)  and  (3.10)  at  the  same  time. 
This  means  in  addition  that 


VW  =  0 


(3.11) 


as  well.  We  know  that  each  of  the  equations 


3W 

3*3 

aw 

2 

aw 

3$! 


Trace 


Trace 


Trace 


3p: 


a<t> 
3  P 


a<j>. 


o 


o 


o 


(3.12) 


has  two  solutions,  one  max  and  one  min.  It  follows  that  (3.11) 
has  eight  (2^)  solutions,  one  for  each  of  the  permutations  of  the 
component  solutions,  and  this  means  one  max,  one  min,  and  six 
saddle  points.  We  require  an  algorithm  that  converges  to  the  max. 

This  can  be  achieved  with  an  iterative  process.  Take  an 
arbitrary  beginning  set  of  values  for  <j>^,  c p.^r  4>^.  All  c£  ^  =  0 
will  do.  Compute  E  for  this  choice  and  then  compute  a  new  ^ 
from  the  first  member  of  (3.9).  Use  this  value  and  the  original 
<j>^  to  compute  F  and  from  this  compute  a  new  <p~  from  the  second 
member  of  (3.9).  Next  use  these  new  values  for  $2  ,  <£3  and 
compute  G,  followed  by  a  new  from  the  third  member  of  (3.9)  . 
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This  completes  one  cycle.  At  each  step  the  appropriate  member 
of  (3.12)  was  satisfied  and  with  a  max.  The  value  of  W  increased  each 
time.  Moreover,  a  second  cycle  beginning  with  the  values  (p^,  and  cf^ 

from  the  first  cycle  would  produce  a  further  increase  in  W.  We 
know  there  exists  a  unique  maximum  for  W  and  we  have  an  iterative 
method  that  increases  W  at  each  step.  It  follows  that,  by 
repeating  the  process,  we  can  come  arbitrarily  close  to  this 
maximum  and  that  will  be  signaled  when  all  three  members  of  (3.12) 
are  negligibly  small  in  magnitude. 

In  order  to  develop  intuition  that  may  be  useful  in  under¬ 
standing  the  more  complicated  optimization  process  addressed  in 
Section  4,  let  us  take  pause  now  and  make  a  heuristic  interpreta¬ 
tion  of  our  iterative  process.  The  objective  function  W  is  a 
sine  wave  as  a  function  of  each  angle  <f>^  (with  the  other  two 
angles  held  fixed) .  As  such  it  is  concave  in  half  of  this  restricted 
angular  domain.  It  follows  that  in  the  full  three-dimensional 
space  of  ( <f>^ , <f>2 *•  ^3)  the  function  W  is  concave  in  one  eighth  of  its 
domain.  A  more  general  (and  perhaps  more  rapidly  converging) 
gradient  search  algorithm  should  begin  in  this  part  of  the 
domain  so  that  when  convergence  takes  Diace,  it  is  toward  a 
maximum.  If  the  algorithm  presented  is  used,  one  does  not  have 
to  worry  about  this  point. 

We  note  in  passing  that  the  above  technique  is  not  limited 

to  three  dimensions.  Suppose  that  our  tracking  problem  was  in 

p  dimensions  and  that  B  was  a  p  xp  orthonormal  matrix.  This 

2 

constraint  reduces  the  degrees  of  freedom,  p  in  B  by  p  for  the 
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length  constraints,  and  by  an  additional  p(p-l)/2  for  the 
orthogonality  constraints.  Since 


p2  -p  -p(p-l)/2  =  p(p-l)/2 

we  see  that  p(p-l)/2  is  the  number  of  angles  (or  equivalently, 
product  matrices  in  the  analogue  of  (3.2))  of  rotation  in  B. 

I.e.,  it  is  the  number  of  ways  we  can  choose  p-2  axes  (from  p 
axes)  to  be  held  fixed  while  a  rotation  takes  place  in  the  planes 
remaining.  The  solution  structure  will  have  one 

P  .  ... 

and  2-2  saddle  points.  Our  iteration  technique 
to  the  max. 


max,  one  min, 
will  converge 
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IV.  GENERAL  CASE 


To  treat  the  general  case  we  must  consider  several  arrays,  say 
K  in  number,  and  several  crossover  regions.  Also  we  must  allow  for 
the  target  to  be  tracked  in  a  given  crossover  region  either  not  at 
all,  exactly  once,  or  more  than  once  since  it  may  maneuver  back 
into  a  given  region  during  a  later  point  count  set.  Finally  the 
target,  during  a  given  point  count  set  in  a  given  crossover  region, 
may  be  tracked  by  more  than  one  sensing  array.  We  proceed  to  develop 
a  notational  structure  that  can  handle  this  fully  general  case. 

Let  S^S^, . . . ,Sp  represent  a  collection  of  R  point  count  sets. 

It  is  convenient  to  assume  that  to  each  individual  S  (for 

s 

s  =  1,...,R)  there  is  associated  a  pair  or  more  of  sensing  arrays. 
Thus,  if  only  one  array  tracks  the  target  in  a  particular  crossover 
region  there  will  be  no  corresponding  point  count  set  in  the  collec¬ 
tion.  If  exactly  two  arrays  track  the  target  then  the  particular 
Sg  is  well  defined.  If  three  or  more  arrays  track  the  target  at 
about  the  same  time,  then  things  become  a  little  fuzzy  because  the 
point  count  set  for  one  pair  of  arrays  may  not  be  exactly  congruent 
with  the  point  count  set  of  the  crossover  data  of  one  of  the  two 
arrays  with  a  third  array,  etc.  This  possible  lack  of  congruence 
does  not  affect  the  computation  of  the  cross  covariance,  (2.12)  as 
they  appear  only  in  pairs.  So  there  is  no  harm  in  allowing  the  sets 
S,,...,S„  to  duplicate  some  segments  of  track.  When  such  duplication 
occurs  however,  the  array  pairs  must  be  distinct. 

Next,  for  i  =  1,...,K,  let  be  the  subcollection  of 

th 

which  has  tracking  data  from  the  i —  array.  In  this 
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way  we  can  identify  a  3-dimension  vector  of  track  data,  X^(t,s), 

til 

as  being  produced  by  the  i —  array  at  point  count  t  which  belongs 
to  Sg.  These  quantities  exist  only  if  Sg  e  C^. 

Our  goal  is  to  estimate  the  displacement  and  re-orientation 
parameter  pairs  for  each  of  the  K  arrays.  Earlier,  with  K  =  2, 
we  learned  that  there  is  an  identif iability  problem  and  it  was 
convenient  to  assume  that  the  location  and  orientation  of  one 
of  the  arrays  was  fixed.  The  same  is  true  in  the  general  case 
(i.e.,  only  one  array  is  fixed)  provided  that  the  data  satisfy  a 
connectivity  condition.  This  condition  can  be  described  in  the 
parlance  of  graph  theory.  The  K  sensing  arrays  form  the  nodes. 

An  arc  exists  between  two  nodes  if  crossover  data  exists  between 
them.  We  wish  to  consider  only  connected  graphs.  What  this 
means  is:  There  is  no  partition  of  the  nodes  into  two  non¬ 
empty  sets  such  that  an  arc  cannot  be  found  connecting  a  node  of 
one  set  to  a  node  of  the  other  set. 

If  this  condition  of  connectedness  is  not  satisfied  by  our 
problem,  then  the  overall  data  must  be  decomposed  into  smaller 
sets  so  that  it  does  hold  for  each  subset.  Such  decompositions 
are  unique  and  each  member  of  the  decomposition  is  to  be  treated 
separately.  This  done  we  can  fix  our  analysis  on  the  case  that 
involves  a  connected  graph.  Here  it  will  be  seen  that  the  estima¬ 
tion  of  all  displacements  and  re-orientation  parameters  will  be 
unique  once  that  one  set  (i.e.,  a  set  corresponding  to  a  given 
array)  is  specified. 

Our  notation  needs  to  be  expanded.  Let  X^(t;s)  be  the  3- 

.  th 

dimensional  track,  in  range  coordinates,  produced  by  the  1 — 


21 


array  at  point  count  t  belonging  to  S  .  Our  goal  is  to  estimate 

b 

the  location  vectors  and  re-orientation  matrices  so  that 
(compare  (2.6) ) 


+  Bixi(t;s)  (4.1) 

are  as  compatible  as  possible  with  the  overall  track  of  the 
target  in  the  range.  Also  let  x^(s)  be  the  vector  of  averages 
of  the  x.  (t;s)  taken  over  t  in  S  ,  and 

^  J.  b 

Xi ( t ; s )  =  xi ( t ; s )  -  ( s )  (4.2) 

be  the  deviations  from  the  mean.  Our  new  objective  function  can 
be  expressed  as  an  extension  of  (2.19) 

Q  =  l  l  l  N  Ave  ||B.X.  (t;s)  -  B.X.  (t;s)  ||  2  (4.3) 

i<j  s  33 

where  the  outer  double  sum  is  over  1  <_  i  <  j  <_  k,  the  inner  sum 

is  over  s  e  C.nC.,  and  the  average  is  to  be  taken  over  t  e  S  . 

1  3  s 

The  weights  Ng  =  number  of  point  counts  in  Ss  for  the  array 
pair  (i, j ) . 

We  can  view  (4.3)  as 

Q  =  Q(B1,B2, . . . ,B  )  (4.4) 

and,  because  of  the  connectedness  assumption,  (4.3)  cannot  be 
decomposed  into  the  sum  of  two  terms 
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Qt  (B.  / • • • ,B.  )  and  Q-, (B  .  > • • • •  ) 

x  X1  xp  31  Dr 

for  which  the  subscript  sets  i^,...,i  and  form  a 

partition  of  1,2,...,K.  Now  we  can  reformat  the  argument  used 
at  the  end  of  Section  2.  If  C  is  any  orthonormal  transformation 
then  Q  (CB-^  ,  CB2  ,  .  .  .  ,  CB  K)  =  Q  (B^  ,  .  .  .  ,BR)  .  Using  C  =  B^,  say,  then 

Q(B1,...,Bk)  =  Q(I,bJb2,  .  •  •  /B^Bk)  (4.5) 

and,  for  convenience,  it  is  assumed  that  the  orientation  of  the 
first  array  is  known,  i.e.,  B^  =  I  in  (4.3). 

Define  covariance  matrices 

Dij (s)  =  Ave{Xi (t;s) xT (t; s) }  (4.6) 

where  the  average  is  taken  over  the  point  counts  t  in  Sg . 

These  quantities  exist  only  for  s  e  CNnCj  f  0.  Then  (4.3)  may 
be  expanded  to  the  more  useful  form 

Q  =  Trace  \  \  \  N  (D..(s)  -  2D .  •  ( s)  bTb  +  D..(s)}  (4.7) 

i<j  s  S  11  13  3  1  33 

and  again,  because  the  unknown  matrices  {B^,}  appear  only  in  the  second 
term,  we  may  choose  to  maximize 

W  =  H  Trace { D. .BTB . }  (4.8) 

■a  13  i  i 

i<j  •  J  J 
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for  1  <  i  <  j  <  K  and 


D..  =  >  N  D..(s)  or  zero 

n  "  s  11 


(4.9) 


according  to  whether  the  summation  for  s  e  nC^  has  content, 
or  is  empty. 

A  word  about  the  computation  of  D^j(s)  in  (4.9).  It  seems 
wise  to  use  the  pooled  within  groups  covariance  in  those  cases 
for  which  Sg  is  the  union  of  rather  diverse  point  counts.  An 
example  should  make  the  point.  Suppose  the  array  pair  (i,j) 
tracks  the  target  at  point  counts  {1,...,25}  and  also  {86,..., 135} 
The  pooled  within  groups  covariance  would  compute  the  two  covari¬ 
ances  separately  and  then  combine  them  into  one  using  a  weighted 
(by  sample  size)  average.  In  our  example,  the  weights  would  be 
25/75  and  40/75. 

For  each  fixed  value  of  r  (r  =  1 , . . . , K)  the  right  hand  side 
of  (4.8)  contains  an  expression  of  the  form 


W  =  J"  Trace{D.  BTB .  }  +  )  Trace {D  .BTb  } 

r  i<r  ir  r  1  j  >r  ^  r 


=  Trace}  J  B . D .  BT  +  7  D  .BTb  } 

i<r  1  ir  r  jir  r:>  3  r 


(4.10) 


To  shorten  the  writing  let 


=  J  B.D. 
.  L  li 


i<r 


ir 


and 


y  d  .b1 

j  >r  J  J 


(4.11) 
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T  T  T  T 

and  use  the  fact  that  Trace{F  B  }  =  Trace{B  F  }  =  Trace{F  B  }. 

3T  3T  X*  3T  X*  I* 

Then 


Wr  =  Tracei (Er  + F^)B^}  (4.12) 

Also  it  is  interesting  to  note  that  each  term  of  (4.8)  appears 
in  two  and  only  two  distinct  W  .  It  follows  that 

k 

l  W  =  2W  (4.13) 

1  r 

Now  the  vector  of  partial  derivatives  of  W  with  respect  to  the 
three  Euler  angles  in  Br,  that  is  (f>  ^ ,  §r2'  ^r3'  same  as 

the  gradient  of  Wr  (with  B^, . . . ,Br_^,B  ^, . . . ,B^  held  fixed). 

Moreover,  the  structure  of  (4.12)  is  the  same  as  that  of  (3.4). 

We  know  from  Section  2  that  VWr  has  eight  zeros,  exactly  one  of 
which  corresponds  to  a  max,  and  we  have  an  algorithm  that  converges 
to  that  max. 

The  analysis  above  leads  to  the  construction  of  a  gradient 

search  that  can  ferret  out  the  max  of  W.  To  fix  one  array  we  set 

B^  =  I  and  keep  this  throughout.  Choose  starting  matrices  for 

the  set  B, ,...,E  .  Compute  E  =  E  (B. , . . . ,B  .)  and 
^  k  r  r  l  r  i 

Fr  =  Fr  (Br+1,  .  .  .  ,Br)  for  each  r  =  1,2,...,K  (E^  =  0  =  FR)-  Use 
the  algorithm  in  Section  2  applied  to  (4.12) ,  for  each  r,  to 
produce  the  new  Euler  angles,  i.e.,  the  system  {B^}.  Use  these 
to  recompute  the  (Er'Fr}  and  repeat.  Stop  when  the  gradients 
of  all  Wr  are  sufficiently  small  in  magnitude.  Each  will 
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be  at  a  locaJ  maximum  for  its  argument  Br  with  all  of  the  other 
orientation  matrices  held  fixed. 

There  are  some  open  questions  about  the  convergence  proper¬ 
ties  of  this  algorithm.  From  an  empirical  point  of  view  it  has 
appeared  to  work  successfully.  Convergence  is  not  monotone  how¬ 
ever.  In  none  of  our  test  problems  have  the  {B^}  departed  notably 
from  identity  matrices,  and  the  use  of  all  B.  =  I  to  initialize 
the  algorithm  has  provided  satisfactory  results.  For  one  run 
the  initialization  was  chosen  at  random  (in  the  3(K-1)  dimensional 
space  of  Euler  angles)  and  the  time  to  convergence  was  excessive. 
We  know  that  W  has  many  saddle  points  and  that  it  is  a  concave 
function  in  only  a  small  part  of  its  domain. 

Methods  for  initializing  the  algorithm  need  to  be  developed. 
The  following  idea  may  hold  some  promise:  Referring  to  each 
term  of  (4.8), 


TraceiD .  .BTb . }  =  TraceiD .  .B . T }  , 

1J  ]  1  ljlj 

T 

where  B* ^  =  B^B^ ,  the  solution  algorithm  of  Section  3  can  be 
used  to  check  whether  B^j  is  near  to  the  identity.  If  so, 
initialize  all  B^  =  I.  If  not,  then  study  of  the  resulting  {B? .  } 
may  lead  to  a  good  selection. 

Let  us  return  to  the  objective  function  Q  of  (4.3)  and  use 
(4.1)  in  it  for  purposes  of  estimating  the  location  parameters 
(Ar>.  Formally  we  have 

Q  =  l  l  l  N  Ave  ||  A.  +  B  X:  (t;s)  -A.  -  B  x  (t;s)  ||  2  (4.14) 

i<j  s  ~  J  J~J 
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where  l<i<j<K;se  C^nC^;  the  average  is  taken  over  the 

points  counts  t  in  S  ;  and  N  =  number  of  point  counts  in  S  . 

s  s  s 

Again  it  is  useful  to  use  terms 


Q  =  y  V  N  Ave  I  A.  +B  •  x.  (t ;  s)  -A  -B  X.  (t;s) 

L,  L  s  H  2T -- i 


i<r  s 


+  y  y  N  Ave  "|  A  +B  x  (t;S)  -  A.  -B.x.  (t;s) 
.  “  s  "  r  r~r  3 


3  <r  s 


(4.15) 


K 

and  recognize  that  l  Q  =  2Q.  In  a  manner  similar  to  the  one 

r=l 

that  produced  (2.7)  we  can  develop 


9Qj 

TKl 


=  -2  y  y  N  {B,  X,  (S)  -B  X  (S)  -  (A  -A.)} 

.  L  L  s  i~ 1  r~r  r  1 


i<r  s 


+  2  y  y  N  {B  x  (s)  -  B.x.(s)  -  (A. -A  )} 
j>r  s  s  r~r  D-D  J  r' 


(4.16) 


Setting  (4.16)  equal  to  zero  results  in  a  3.K  linear  system  in 

the  Aw  .  .  .  ,  A T,.  Let  M.  .  =  j  N  for  s  e  C .  n  C  •  .  Then  the  left 
1  K  13  s  13 

J  s 

hand  side  of  the  linear  system  is 


y  m.  a.  +{  y  m.  +  y  m  .}a  -  >  m  .a. 

•  ir  1  lr  ^ r3  r  n  i 


i<r 


i<r  3>r 


>r  r^>  3 


(4.17) 


and  the  right  hand  side  is 
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X  o  ~  -J- 

i<r  s 


I  Bj_  l  NsXi(s)  + 


(4.18) 


Notice  that  the  coefficient  matrix  in  (4.17)  is  the  same  for  all 
three  components  of  the  {Ar}.  Also  the  columns  of  the  matrix 
add  to  zero  so  that  its  rank  is  K-l.  It  follows  that  the  system 
is  underdetermined.  We  anticipated  this.  Setting  A-^  =  0  will 
allow  a  unique  solution  for  A2»-../AK.  Of  course,  this  choice 
corresponds  to  the  assumption  made  earlier  that  the  location  and 
orientation  of  the  first  array  is  known. 
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V.  PRINCIPAL  COMPONENTS  METHOD 


Experience  with  the  least  square  method  (two  arrays,  Section 
2)  in  the  area  of  underwater  tracking  has  revealed  some  instabili¬ 
ties.  Several  cases  were  identified  in  which  the  surface  W  con¬ 
tained  some  rather  extensive  relatively  flat  regions  as  a  function 
of  the  three  Euler  angles.  More  specifically,  it  is  possible  to 
move  from  the  maximizing  point  to  a  saddle  point  and  lose  less 
than  one  percent  of  the  value  of  W.  Moreover  the  effect  of  using 
the  maximizing  point  can  be  to  assign  an  absurd  relocation  geometry 
to  the  position  of  the  sensor  (e.g.,  turned  upside  down  and  sus¬ 
pended  without  support  somewhere  in,  or  above,  the  water).  In 
these  cases  use  of  the  saddle  point  involved  only  minor  relocation 
of  the  sensor.  Clearly  this  is  an  unsatisfactory  state  of  affairs. 

These  anomalies  can  happen,  and  there  is  not  yet  any  fully 
reliable  way  to  identify  the  conditions  under  which  they  may 
occur.  They  seem  to  be  related  to  the  inherent  variability  of 
the  tracking  data  coupled  with  the  point  by  point  crossover  data 
matching  approach  utilized  by  the  least  square  methodology.  It 
may  be  that  certain  naturally  occurring  geometric  conditions 
(e.g.,  straight  line  structure  of  track)  may  act  as  a  catalyst 
for  this  phenomenon. 

An  alternative  methodology  has  been  developed  and  it  appears 
to  avoid  producing  the  absurd  solutions  mentioned  above.  It  is 
based  on  the  matching  of  the  first  principal  components  of  the 
covariance  matrices  of  the  tracking  data. 
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Let  us  take  a  moment  to  describe  the  notion  of  first  princi¬ 
pal  component.  Suppose  we  want  to  fit  a  segment  of  straight 
line  to  a  track  of  data  {x(t);t  eS}.  Further  suppose  we  use  the 
minimum  Siam  of  squared  projections  (orthogonal)  as  the  mathemati¬ 
cal  criterion  for  choosing  the  line.  The  solution  is  well  known 
(Ref.  1,  p.  272  ff ) .  Let  A  be  the  largest  eigenvalue  of  the 
covariance  matrix  D  and  let  q  be  an  eigenvector  associated  with 
A.  That  is,  any  solution  of 

Dxxq  =  Xq  (5.1) 

The  following  facts  are  stated  without  proof: 

(i)  The  vector  q  is  a  set  of  direction  numbers  for  the 
desired  straight  line. 

(ii)  The  line  passes  through  the  centroid  x. 

(iii)  The  projected  track  of  x(t)  onto  this  straight  line 

T 

are  the  scalar  values  u(t)  =  q  x(t). 

These  properties  can  be  exploited  for  our  current  problem  in 
the  following  way.  Let  p  be  a  first  principal  component  for  D 
and  q  play  the  same  role  for  D  .  Both  are  assumed  to  be  scaled 
to  have  length  one: 


Adopt  as  the  optimizing  criterion:  Choose  B  orthonormal  so  that 

p  =  Bq  (5.2) 


30 


That  is,  the  re-orientation  matrix  is  chosen  so  that  the  two 
fitted  straight  line  segments  have  the  same  direction  numbers. 

It  appears  that  the  mathematical  problem,  p  =  Bq,  is  most 
easily  solved  using  a  constructive  method.  If  p  =  q  then  B  =  I. 
Barring  this,  proceed  as  follows: 

1)  Let  0  be  the  angle  between  p  and  q. 

2)  Form  an  orthonormal  basis  H  =  {h^h^/h^}  such  that  h^  =  q; 
h2  is  in  the  plane  of  p  and  q  but  orthogonal  to  h^; 

h^  completes  the  basis. 

/  2 

3)  Letting  c  =  cos(0)  and  s  =  »l  -c  ,  we  can  use  the  Gram 
Schmidt  process  and  take 

=  (q  -cp) 


4)  Notice  that 


Hq  = 


and 


Hp  = 


5) 


and  that  Hp  can  be  rotated  through  an  (Eulerian)  angle 

T 

0  into  Hq  by  applying  the  matrix  p^(0),  see  (3.2). 

Since  p^Hp  =  Hq  and  both  p^  and  H  are  orthonormal  matrices, 


T  T 


it  follows  that  p  =  H  p^Hq. 


Thus 


T  T 

B  =  H  P3H 


(5.3) 


Notice  that  this  method  does  not  require  crossover  data  in 

the  strict  sense,  i.e.,  matched  pairs  of  track  from  two  arrays 

for  a  common  set  of  point  counts.  The  matrix  D  is  not  needed 

yx 
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and  D  and  D  can  be  computed  without  this  requirement, 
yy  xx 

However  the  stability  of  the  principal  components  p  and  q 
require  that  the  two  pieces  of  track  be  rather  straight,  i.e., 
no  curvilinear  bias. 

If  the  input  data  are  not  crossover  data,  there  must  be  an 
adjustment  in  the  way  that  we  estimate  the  displacement  parameter 
A.  The  formula  (2.9)  will  not  be  valid  because  the  x  and  y 
values  do  not  refer  to  the  same  time  (i.e.,  point  count).  Our 
immediate  goal  is  to  estimate  the  distance  6  traveled  by  the 
target  from  its  mean  position  y  at  some  time  t  to  its  new  esti¬ 
mated  position  A  + Bx  at  some  later  time  t  .  Let  us  assume  that 
the  two  point  count  sets  (one  for  the  {y}  and  one  for  the  {x}) 
are  mutually  exclusive  and  represent  equally  spaced  time  values. 
Since  the  target's  path  is  straight  we  can  average  the  times  in 

the  two  point  count  sets  to  obtain  values  for  t„  and  t  .  Then 
c  y  x 

one  can  form  the  sets  of  projections 

u ( t )  =  qTx(t)  and  v(t)  =  pTy(t)  (5.4) 

and  use  their  successive  differences  to  get  an  estimate  of  the 
target's  speed. 

The  distance  6  will  be  the  speed  multiplied  by  the  time 

increment  t  -  t  . 

x  y 

This  done,  it  is  claimed  that 

A=y-Bx+6p  (5.5) 
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Proof:  The  lineal  distance  6  can  be  represented  as 


p^  (A  +  Bx  -  y )  =  6 

T  T  —  — 

from  which  it  follows  that  p  A  =  p  (y  -  Bx)  +6.  We  can  view  p 

as  the  first  column  of  a  seld  adjoint  matrix  P,  and  letting 
T 

6  =  (6,0,0)  we  can  express  our  equation  in  the  form 

PTA  =  PT(y-Bx)  +  6  (5.6) 

From  (5.6)  it  follows  that  A  =  (y  -Bx)  +  P6  and  this  is  the 
same  as  (5.5). 

It  should  be  noted  that  the  principal  components  method  could 
have  been  used  in  place  of  the  least  squares  approach.  Having 
crossover  data  in  hand  does  not  preclude  its  use.  Thus  t  =  tx 
and  6  =  0  in  (5.5) .  We  can  modify  the  objective  function  Q  of 
(2.11)  and  get  a  perfect  fit,  i.e.,  because  of  (5.2), 

0  =  ||  P  -  Bq  j  |  =  pTp  -  2pTBq  +  qTq 

=  2-2  Trace{pqTBT}  (5.7) 

and  the  Trace  factor  of  (5.7)  is  unity. 

With  the  above  in  mind  let  us  turn  to  the  question  of  extend¬ 
ing  the  principal  components  method  to  several  arrays  and 
several  crossover  regions.  It  is  simpler  if  we  have  crossover 
data  so  let's  describe  that  situation  first. 
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The  notation  is  the  same  as  in  Section  4.  Let  qL(s)  be 
the  first  principal  component  of  D^(s)  (see  (4.6)).  The 
orientation  changes  {B^}  will  be  selected  first  by  minimizing 


I  I 
i<j 

I  l 
i<j 


l  ||qi(s)B.  -qT(s)B.  ||  2 

S  J  J 

X { 2  —  2  Trace [q^ (s) (s) bTb^]  } 

Q  J  J 


(5.8) 


and  the  summations  are  for  1  £  i  <  j  _<  K  and  s  e  C^nCj.  As 
before,  this  is  tantamount  to  maximizing 


W 


J  y  Trace(D. .BTB . }  with  D.. 

ID  D  1  ID 


I  qi(s)qT(s)  (5.9) 
s  J 


Since  this  has  the  same  structure  as  (4.8),  the  same  solution 
algorithm  can  be  used.  Also  the  system  (4.17)  and  (4.18)  can  be 
used  to  obtain  the  {A^}. 

The  procedure  changes  a  bit  when  the  data  are  not  of  the 
crossover  form.  That  is,  when  we  are  splicing  together  segments  of 
track  and  there  are  no  overlaps.  One  might  liken  this  problem 
as  being  similar  to  putting  in  water  pipes  in  a  large  building. 
Several  subcontractors  have  put  in  their  pipes  and  our  job  is  to 
connect  it  all  up.  The  pieces  must  be  moved  so  that  the  ends 
butt  up  against  one  another. 

With  this  view,  we  need  a  change  in  terminology.  Let 
S.,...,S„  represent  the  crossover  points,  or  connection  points. 

To  each  Ss  there  corresponds  two  arrays,  i.e.,  an  unique  (i,j) 
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pair.  Let  TV(s)  and  Tj  (s)  be  the  point  counts  that  produce  the 
data.  It  is  assumed  that  they  do  not  overlap.  Each  is  suffi¬ 
ciently  extensive  so  as  to  provide  stable  estimates  for  the 
covariance  matrices  D^^(s)  and  Djj(s),  yet  not  so  extensive  as 
to  compromise  the  assumption  that  the  target  is  moving  in  a 
straight  line  while  in  the  point  counts  neighboring  the  connec¬ 
tion.  This  done,  we  can  input  equation  (5.9)  and  use  the  algorithm 
of  Section  4  to  estimate  the  {B^}. 

It  remains  to  estimate  the  {A^}.  To  do  this  the  technique 
preceeding  equation  (5.5)  needs  to  be  expanded.  With  more  than 
two  arrays  involved,  the  objective  function  Q  of  (5.8)  will  not 
be  zero.  This  means  that  our  reoriented  segments  of  track  will 
not  share  an  exact  common  line  at  the  connection  points.  With 
this  possibility  in  mind  we  introduce  some  further  quantities. 

To  each  of  the  sets  T^(s)  and  Tj(s)  adjoin  (if  necessary)  a 
common  time  value  (or  pseudo  point  count)  t^j (s)  which  will  serve 
as  the  connecting  time.  Let  t^(s)  and  tj  (s)  be  the  average 
values  of  Tx(s)  and  T^ (s)  respectively.  For  convenience  it  is 
assumed  that  the  former  preceeds  the  latter;  t^(s)  <  tj (s) .  The 
corresponding  pre-adjusted  positives  are  x^(s)  and  Xj (s) .  Esti¬ 
mate  the  target's  speed  in  the  same  array  as  before  and  use  it 
to  compute  the  distances: 


6^(s)  =  distance  traveled  from  t^(s)  to  t^j (s) 


6 j (s)  =  distance  traveled  from  t^j  (s)  to  tj(s) 
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These  will  be  used  in  our  immediate  goal,  namely,  to  take  the 
linearly  adjusted  pieces  of  track 

A 

X^(t;s)  =  Ai+Bixi(t;s) 


(5.10) 

A 

Xj  (t ;  s)  =  A  j  +BjXj(t;s) 


and  model  the  differences  A.  -A.  in  terms  of  the  6. (s) ,  <5. (s) 

3  i  l  j 

and  other  known  quantities. 

In  continuing,  let  us  drop  the  argument  s  from  the  notation. 
Again  we  can  use  the  eigenvectors,  q^,  to  project  the  track  to 
an  axis  so  we  can  measure  the  distances.  Specifically,  write 


qTx.  (t.  .)  -  qT[A-  +B.  x.  ] 


6  . 

l 


qj1Aj  ♦Vi1  '  qjxj<tij) 


=  6. 


(5.11) 


Although  we  don't  know  how  to  compute  x^(t^j)  and  Xj(t^j)  they 
both  represent  the  position  at  the  connecting  point, 

/V  /s. 

xi^ij^  =  xj^t^j).  Next  rewrite  (5.11)  as 


T 

q  .  A . 
D 


T  — 

q  .B  .  x  .  +  6  . 
D  D  D  D 


T  _ 

q  .  A. 

l 


q1xi<tij 


;)  - 


T  — 

q  .  B  .  x.  -  6  . 


(5.12) 


Then  apply  the  same  technique  as  used  in  the  proof  of  (5.5). 
The  result  is 
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The  unknown  quantities  x^(t^j)  will  cancel  when  we  take 
differences.  Let  us  restore  the  s  to  the  notation  and  record 
the  result. 

A_.  -Ai  =  Bixi(s)  -  BjXj(s)  +  6  j  (s)q  •  (s)  +  5i(s)qi(s) 

(5.14) 

The  system  (5.14)  is  overdetermined  and  has  no  solution.  Let  us 
develop  the  least  squares  compromise.  To  shorten  the  writing  let 
gij(s)  represent  the  right  hand  side  of  (5.14).  Many  details 
will  be  omitted  because  we  follow  the  pattern  at  the  end  of 
Section  4. 

Let 


Q 


l  l  I  II  A, 

i< j  s  J 


(5.15) 


and 


•III 

i<r  s 


A  -  A .  -  g  .  .  (s  ) 

i  r  3_  ^  i  j  ' 


2  +  l  I  l  llAi  -  Ar  -gri  (s)  II 2 

j>r  s  J  J 


Then 


(5.16) 


I  l  Z{Ar-A± 


-g 


ir 


(s)  } 


-  I  l  UK  -Ar-9r1(s)} 
j  >r  s  J  J 


which  is  set  equal  to  zero  for  each  r  = 

the  number  of  (i,j)  connections  that  exist,  i.e., 

and  proceed.  For  r  =  1,...,k  we  have  the  system 


Let 


nij 


be 

1, 


4?_  nirAi  +  VA  nir  +  J_  nri  }  "  A  nrjAj 


i<r 


i<r 


j  >r  rD  j  >r 


.1  I  9ir  (s)  "  I  I  9ri <s) 

i<r  s  j  >r  s  J 


(5.17) 


and  the  9£r(s)  and  grj (s)  can  be  found  from  the  right  side  of 
(5.14).  As  before,  the  columns  of  the  coefficient  matrix  add  to 
zero  and  the  rank  of  the  system  is  K-l.  We  set  =  0  (i.e., 

the  location  of  the  first  array  is  assumed  known)  and  solve. 
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APPENDIX 
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SUBROUTINE  CALL  CHART 


*  * 
* 
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CALLED  AT  TWO  DIFFERENT  PLACES  IN  THE  CALLING  SUBROUTINE 
CALLED  AT  FIVE  DIFFERENT  PLACES  IN  THE  CALLING  SUBROUTINE 


oo  o  ooooo  o  o  o  o  o  o  o  ooooooooooooo 


PROGRAM  KEYMAIN 

*************************************************************** 
***  This  serves  as  a  main  program  to  call  the  subroutines  *** 
***  used  to  estimate  the  sensor  array  displacements  and  *** 

***  the  array  re-orientation  angles  for  the  Keyport  range  *  *  * 


***  calibration  project.  It  reads  In  data  from  a  disk  *** 
***  file  specified  by  the  user.  It  prompts  the  user  for  *** 
***  this  data  and  for  sensor  array  I.D.  Information.  *** 
***  --  26  August  1985  *** 


a########################**###*****#*##################*####*#* 


...  Declare  all  integer  variables: 

INTEGERS  IND2 ( 2 , 3  0 ) ,  IND1(30,30),  Rl,  NUMREC,  I,  J,  IDL, 

2  K,  I A ( 3 0 ) ,  TESTC,  NS1(30),  IND(30,30),  R,  NS(30), 

3  FF(30,30),  IK,  KM,  IDR,  IND2R(2,30),  DATSET (30), 

4  CHOICE,  OUT,  IN 

...  Declare  all  real  variables  in  double  precision: 

RE AL*8  AA ( 200 , 6 ) ,  CROSSA ( 3 0 , 3 , 3 ) ,  MEAN(30,6),  DEV(30,3,3), 

2  XB  (  3  0 , 6  )  ,  EP,  BOO, 3, 3),  BB(3,3),  XBB(30,6),  A(30,3), 

3  DEL  (30,3),  D  ( 3  0 ) ,  POO),  COO),  RM(6),  PP,  EA(3C,3) 

...  Declare  the  character  variables: 

CHARACTER  DSNAME*13 ,  ANSWER*3 

LOGICAL  EXST 

PARAMETER ( 0UT=6 , IN=5 ) 

WRITE(QUT,*)  1  This  is  a  user  friendly  program.  Hello  !!.  * 

WRITE ( OUT , * )  '  ' 

Rl  =  0 

...  The  IND2  and  IND2R  matrices  serve  to  store  array  pair 

identification  information  and  relate  it  to  tire  input 
(six  column)  crossover  data  sets. 

...  Initialize  IND2  and  IND2R  matrices  to  zero: 

DO  30  I  =  1,2 
DO  30  J  =  1,30 
IND2 ( I , J )  =  0 
IND2R ( I , J )  =  0 
30  CONTINUE 

5  W RITE ( OUT, * )  ’  Please  enter  the  name  of  the  data  set  on  disk:  * 

...  Rl  counts  the  number  of  cata  sets  input  by  the  user. 

Rl  =  Rl  +  1 
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ooo  o  o  o  o  o  o  o  oo  ooooo 


READ( IN, ' (A)  ' )  DSNAME 

C  ...  Check  to  make  sure  data  file  exists: 

INQUIRE ( FILE® DSNAME, EXIST* EXST) 

I F ( . NOT .  EXST)  THEN 

WRITE(0UT, *) '  The  file  does  not  exist.' 

9  WRITE ( OUT, *) '  Do  you  want  to  (1)  try  again  or  (2)  abort?' 

READ( IN, *)  CHOICE 
I F ( CHOICE  . EQ.  1)  THEN 
R1  =  R1  -  1 
GOTO  5 

ELSE  IF (CHOICE  .EQ.  2)  THEN 

WRITE ( OUT, #) '  Program  terminating  at  your  request.' 
STOP 

ELSE  IF (CHOICE  ,NE.  1  .AND.  CHOICE  .NE.  2)  THEN 
WRITE (OUT,*)'  Please  enter  Integer  1  or  2  .  ' 

GOTO  9 
END  IF 
END  IF 


OPEN ( 1, FI LE= DSNAME, STATU S= 'OLD' ) 

...  Read  data  from  file  and  count  number  of  records  (NUMREC): 
NUMREC  =  0 

10  NUMREC  =  NUMREC  +  1 

READ ( 1, *, END=20, ERR=15 ) (AA (NUMREC, J ) , J=1 ,6 ) 

GOTO  10 

...  Notify  operator  of  file  read  error: 

15  WRITE ( OUT, * ) '  » 

W R ITE ( OUT, * ) '  An  error  was  detected  in  reading  the  file,' 

WRITE (OUT,*)'  but  execution  will  continue  anyway  ...' 

WRITE (OUT, *) '  ' 

WRITE ( OUT , * ) '  ' 

20  NUMREC  =  NUMREC  -  1 

WRITE(OUT,*)  '  There  are  ', NUMREC,'  records  1 1 .  data  set  '  ,  D  S  f  i  F  ME 
CLOSE ( UN IT= 1 ) 

...  The  number  of  records  per  input  data  set  are  accu.  elated 
in  the  vector  NS1. 

NSl(Rl)  =  NUMREC 


...  Define  left  c.  right  sensor  no.'s  for  matrix  IKD2: 

"left"  will  be  indexed  with  a  one  arid  "  r  i  0  i,  t "  will 
be  indexed  with  a  two. 

WRITE ( OUT, * )  '  Name  your  left  sensor  array  number:  ' 

READ ( IN, *)  IDL 

WRITE ( OUT, * )  '  Name  your  right  sensor  array  number:  ' 

READ (IN,*)  IDR 

...  The  left  and  right  array  ID's  are  entered  i n  the 
first  arid  second  (respectively)  rows  of  I N D 2 . 

I ND2  (  1 ,  P.l )  =  IDL 
IND2 ( 2  >  Rl )  =  IDR 
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ooooooooooooooooooooo  oo  ooooooo 


...  Begin  subroutine  calls: 

Subroutine  CROSSP  takes  each  Input  data  set  AA,  which 
has  NUMREC  records*  and  returns  RM  -the  six  component  mean 
vector  and  C-the  three  by  three  matrix  of  sums  of  crossprocu 
deviations  from  the  means.  These  are  computed  sequentially 
and  stored  in  the  arrays  MEAN  and  CROSSA  as  they  are  compute 

CALL  CROSSP(NUMREC, AA* RM,C) 

DO  50  I  =  1,3 
DO  50  J  =  1,3 
CROSSA ( R1 , I , J )  =  C(I,J) 

50  CONTINUE 

DO  60  KM  =  1,6 
MEAN ( R1 , KM )  =  RM(KM) 

60  CONTINUE 


V/  RITE  ( OUT,  * )  '  Would  you  like  to  call  another  data  set  ?  ' 

READ( IN,  '  (A) * )  ANSWER 

IF  (ANSWER  .EQ.  ’YES’  .OR.  ANSWER  . EQ.  ’Y’)  GOTO  5 

IF  (ANSWER  .EQ.  'yes’  .OR.  ANSWER  .EQ.  ’y’)  GOTO  5 

...  The  subroutine  CONECT  (Gygax)  takes  as  input  the  number 

of  data  sets  Rl,  and  the  left/right  array  ID  matrix 
IND2  and  determines  whether  the  problem  input  by  the 

user  is  connected.  That  is,  all  sensor  arrays  in  the 

problem  communicate  with  one  another  via  a  string  of 
crossover  data  sets.  If  this  test  fails,  tier:  tie 
output  variable  TESTC  is  zero  and  the  user  is  prompted 
to  use  one  of  the  three  options  listed  in  the  WRITE 
statements  bel ow . 

...  If  the  problem  is  connected,  then  TESTC  is  not  zero  a r. C 
the  subroutine  returns  a  value  for: 

K,  the  number  of  sensor  arrays  in  the  problem. 

IND1,  a  useful  conversion  of  IND2. 

IA,  the  list  of  sensor  ID’s  in  the  order- 
maintained  by  the  program. 

IND2R,  the  submatrix  of  IND2  that  represents  eutc  set 
connected  to  the  first  data  set. 

DATSET,  the  number  of  crossover  data  sets  conrocito 
to  the  first  input  data  set. 

CALL  CONECT (OUT, Rl , IND2, K, IND1 , I A, TESTC, IND2R, DATSET) 

IF  (TESTC  .EQ.  0)  THEM 

WRITE ( OUT, * )  ’All  of  your  arrays  are  not  connected.  ’ 

WRITE ( OUT, * )  ’Do  you  want  to  :’ 

WRITE (OUT , * )  '  (1)  Quit  now’ 

WRITE ( OUT , * )  ’  (2)  Input  more  data’ 

WRITE(0UT»*)  ’  (3)  Continue,  using  the  first  connected  set’ 

65  WRITE (OUT,*)  'Please  enter  the  number  of  your  choice  ' 

READ ( IN, * )  I 

IF  (.NOT.  ((I  .EQ.  1 ) . OR . ( I  .EQ.  2). OR. (I  .EQ.  3)))  GOTO  65 
GOTO  (66,  67,  66), I 

66  WRITE(0UT,*)  'Program  te r m i n a t i n g .  Adios,  Amigo  !' 

STOP 
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67  GOTO  5 

...  The  subroutine  REDUCE  (Gygax)  does  nothing  unless  option  3 
above  Is  evoked.  In  that  case  It  modifies  the 
variable  CROSSA,  MEAN,  Rl>  K>  INDl,  and  IA  so  that 
they  contain  only  the  Information  required  by  the 
connected  problem  called  for  by  this  option. 

68  CALL  REDUCE(CROSSA, MEAN, R1,K, INDl, IA,IND2R,DATSET) 

END  IF 

...  Feed  the  arrays  output  by  CONECT  or  REDUCE  Into  POOL: 

The  subroutine  POOL  checks  on  the  number  of  data  sets 
associated  with  each  pair  of  sensor  arrays.  If  this 
value  Is  one  or  zero  it  does  nothing.  Otherwise  it 
(in  effect)  pools  all  the  data  associated  with  each 
unique  sensor  array  pair  into  one  data  set  (using  trie 
within  groups  sum  of  squares  technique  for  crossprocucts, 
ana  the  weighted  average  technique  for  means.  Thus* 
CROSSA  is  converted  to  DEV;  MEAN  to  XD;  R1  to  R;  INDl  to 
IND;  and  NS1  to  NS.  The  first  six  called  variables  are 
Input  and  the  last  five  are  output. 

CALL  POOL ( K, Rl, INDI, CROSS A, MEAN, MSI, IND, DEV, X6,R, NS) 

...  Array  IND  output  by  POOL  goes  into  INVIND: 

The  subroutine  INVIND  takes  the  variables  K ,  R>  a  n  c  ZiiD 
and  outputs  the  K  by  K  matrix  FF.  This  is  an  u^per 
triangular  incidence  matrix  which  contains  a  one  in 
the  position  i » j  (  i  <  j )  only  if  there  exists  a  set  of 
crossover  data  involving  both  the  1-th  and  j-ti 
sensors  (as  indexed  in  IA).  FF  is  essentially  an 
inverse  of  IND. 

CALL  INVIND (K, R, IND, FF) 

...  Array  FF  is  output  from  INVIND  and  input  to  MULTAR. 

...  Set  epsilon,  the  tclerafcle  error  level  used  in  the 
stopping  rule  of  the  subroutine  MULTAR: 

EP  =  l.D-6 

...  The  subroutine  MULTAR  computes,  cy  iteration,  the  K 

orthonorn.al  re-orientation  matrices  for  each  sensor 
listed  in  IA.  Since  these  are  all  relative  to  the 
first  sensor,  the  first  matrix  in  the  output  B  is 
the  3  x  3  identity  matrix. 

CALL  MULTAR(EP,FF,DEV,  K , R, B ) 

...  Array  B  is  output  from  MULTAR  ana  incut  to  INTCEP: 

CALL  INTCEP(FF,NS,B,XB,K,P.,  A) 
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...  Array  A  Is  output  from  INTCEP  and  Input  to  DISPLC: 

...  The  subroutine  DISPLC  takes  variables  K»  B»  A,  ana  IA  as 

Input*  reads  the  sensor  array  location  file  and  computes 
the  estimated  displacement  of  each  of  the  K  sensors  in 
the  problem.  The  output  DEL  Is  the  set  of  d 1 s p 1 aceme nt 
vectors  and  D  is  the  corresponding  set  of  K  displacement 
distances.  Since  these  are  all  relative  to  the  first 
sensor*  the  first  row  of  DEL  and  first  element  of  D  are 
zero. 

CALL  DISPLC(K,B,A, IA, DEL , D ) 

...  Prepare  the  sequential  computation  of  the  maximum  angles 
of  sensor  rotation. 

DO  75  IK= 1 >  K 

DO  70  1=1,3 

DO  70  J  =  1 , 3 

70  BB( I, J )  =  B( IK, I, J ) 

...  Array  BB  is  input  to  MAXANG 

...  The  subroutine  MAXANG  takes  a  3  by  3  o r t ho  no rma 1  matrix  BE 
and  computes  PP,  the  largest  possible  rotation  angle 
experienced  by  any  vector  transf ormed  by  BB. 

CALL  MAXANG ( BB, PP ) 

P ( IK )  =  PP 
75  CONTINUE 

...  The  subroutine  ANGLES  converts  each  of  the  K  ortho  normal 
matrices  in  B  to  their  representation  as  the  three 
Euler  angles:  Roll,  Pitch,  and  Yaw.  Since  these  are 
relative  to  the  first  sensor,  the  first  row  of  this 
matrix  is  zero. 

CALL  ANGLES ( K, 6, EA) 


...  Prepare  to  write  the  output  to  files: 

OPEN (3, FILE= 'KEYFIL1 . DAT' , STATU S= 'OLD  » ) 

OPEN (4, FILE* 'KEYFIL2.DAT' , STATU  S= '  CLD ' ) 

0PEN(7,FILE=' KEYFIL3.DAT' , STATU S= 'OLD • ) 

OPEN (8, FILE*' KEYFIL4.DAT* , STATUS* 'OLD' ) 

WRITE ( OUT, * )  '  D  i  sp 1 acement  and  rotation  (magnitude):' 

DO  77  IK  =  1 , K 

...  Write  the  vectors  IA,  D  and  P  to  FILE1 
WRITE (3,85)  I A ( I K ) ,  D(IK),  P ( I K ) 

WRITE (OUT, 85 )  IA(IK),  D(IK),  P  (  I K ) 

...  Write  the  vector  IA  and  matrices  A  and  DEL  to  FILE2. 
WRITE (4,80)  IA(IK),  (DEL ( IK, L) , L=1 ,3 ) ,  ( EA( IK, LK ) , LK= 1 , 3 ) 
77  CONTINUE 
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WRITE(0UT,*)  '  Displacement  vectors  and  Euler  angles  s' 

DO  78  IK  =  1,K 

WRITE ( OUT, 80 )  I A { IK) ,  ( DEL ( IK , L ) , L= 1 , 3 ) ,  ( E A ( IK , LK ) , LK= 1 , 3 ) 

78  CONTINUE 

80  FORMAT ( IX, 15, IX, 3 F 12. 4, IX, 3 (IX, FI  0.7) ) 

85  FORMAT (2X, I5,2X,F10.2,2X,F10.6) 

...  Write  the  three  dimensional  array  B  to  FILE3. 

WR ITE ( OUT, * ) '  ' 

WRITE ( OUT, * )  '  Array  B  output  by  KEYMAIN:  ' 

DO  90  IK  =  1 ,  K 
WRITE ( OUT ,  * )  '  ' 

WRITE (OUT,*) '  ' 

DO  90  I  =  1,3 

WRITE (7, 100) (B(IK,I,J),J=1,3) 

WRITE (OUT, 100) (B(IK,I,J),J=1,3) 

90  CONTINUE 

100  FORMAT (2X,3F15 . 10) 

DO  105  IK  =  1 , K 

WRITE (8, 115 ) ( A( IK, J ) , J-1,3 ) 

105  CONTINUE 

115  F0RMAT(2X,3F15.5) 

WRITE(0UT, 110) 

110  F0RMAT(//,'  Program  terminating.  Operation  complete.  Eye.  ') 

CLOSE ( UN IT=3 ) 

CLOSE ( UNIT=4 ) 

CL0SE(UNIT=7) 

CLOSE ( UNIT*8 ) 


STOP 

END 


SUBROUTINE  CROSSP  ( M , AA , ME  AN , CROS S A ) 

*«*******»*****i***H#*****»****«**»*****ini*iHHHi**it**»ini*i(m*H** 

***  THIS  PROGRAM  CALCULATES  THE  CROSSPRODUCT  DEVIATIONS  FOR  ** 

***  THE  RAW  DATA  ARRAY  AA  (A  MATRIX  FOR  EACH  CROSSOVER  SET) 

***  THE  SET  OF  MEANS  IS  DEVELOPED  AND  LABELED  "MEAN"  ** 

***  CROSSA  IS  THE  SUM  OF  CROSSPRODUCT  DEVIATIONS  FROM  THE  MEAN  3  ** 
###*###**#*####*■***######**#*##**##**###*#*########  *************** 

INTEGERM  N 

REALMS  AA ( 200 , 6 ) ,  MEAN ( 6 ) ,  CROSSA(3,3),  SUM 

RE AL *8  SUKXX ,  SUMXY,  SUMXZ,  SUMYX,  SUMYY ,  SUMYZ,  SUMZX,  S'JMZY 
RE AL  *  6  SUMZZ ,  XX,  XY,  XZ,  YX,  YY,  YZ,  ZX,  ZY,  ZZ 

...  N  is  the  number  of  rows  in  the  data  matrix  A  A 
...  Compute  the  vector  of  means. 
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DO  20  J  =  1,6 
SUM  =  0 . DO 
DO  10  I  =  1,N 
SUM  =  SUM  +  AA( I, J ) 

10  CONTINUE 

MEAN ( J )  =  SUM  /  DBLE(N) 
20  CONTINUE 


...  Begin  the  crossproduct  operations: 

Initialize  the  nine  sums  to  zero. 

SUMXX=0 . DO 
SUMXY=0 . DO 
SUMXZ=0 . DO 
SUMYX=0 . DO 
SUMYY=0.D0 
SUMYZ=0 . DO 
SUMZX=0 . DO 
SUMZY=0 . DO 
SUMZZ=0 . DO 

DO  100  IA  =  1 , N 

...  Create  the  crossproduct  deviations  for  row  IA  of  matrix  AA 

XXs ( AA ( I A, 1 ) -MEAN ( 1 ) ) * ( AA ( I A, 4 ) -MEAN ( 4 ) ) 

XY= ( AA( IA, 1)-MEAN( 1) ) * ( AA ( I  A, 5 ) -ME  AN ( 5 ) ) 

XZ= (AA( IA, 1)-MEAN( 1) ) * ( AA ( I A , 6 ) -ME AN ( 6 ) ) 

YX=(AA( IA, 2) -MEAN (2) ) *(AA( IA, 4 ) -ME AN ( 4 ) ) 

YY=(AA(IA,2)-MEAN(2) ) * ( AA ( I  A, 5 ) -MEAN ( 5 ) ) 

YZ= ( AA ( IA,2)-MEAN(2) )*(AA( IA,6)-MEAN(6) ) 

ZX=(AA(IA,3)-MEAN(3) ) * ( AA ( I  A, 4 ) -ME AN ( 4 ) ) 

ZY=(AA(IA,3)-MEAN(3) )#(AA(IA»5)-MEAN(5) ) 

ZZ=(AA(IA,3)-MEAN(3) ) * ( AA ( I  A, 6 ) -MEAN ( 6 ) ) 

...  Accumulate  the  sums: 

SUMXX=XX+SUMXX 
SUMXY*XY+  SUMXY 
SUMXZ-XZ+SUMXZ 
SUMYX= YX+SUMYX 
SUMYY= YY+SUMYY 
SUMYZ=YZ+SUMYZ 
SUMZX=ZX+SUMZX‘ 

SUMZY=ZY+SUMZY 
SUMZZ=ZZ+SUMZZ 
C 

100  CONTINUE 
C 
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C  ...  Create  the  CROSSA  matrix: 

CR0SSA(1,1)=SUMXX 
CROSS A ( 1*  2)=SUMXY 
CR0SSA(1,3)=SUMXZ 
CR0SSA(2,1)=SUMYX 
CR0SSA(2,2)=SUMYY 
CR0SSA(2,3 )=SUMYZ 
CR0SSA(3 , 1)=SUMZX 
CR0SSA(3 ,2)=SUMZY 
CROSSA(3,3)=SUMZZ 
C 

RETURN 


C 


END 


SUBROUTINE  CONECT  ( OUT, R1 , IND2, K, IND1 , IA, TESTC, IND2R,  DATSET) 


************************************#*******#*****x**x********** 


***  This  subroutine  checks  for  the  connectedness  of  the 
***  input  data  sets.  If  the  problem  is  connected  then  the 
user  is  Informed  and  the  array  pairs  are  printed  on  the 
screen;  if  not  connected,  then  the  user  is  prompted  to 
select  one  of  three  options  -  quit,  add  conecting  data 
sets,  or  run  the  program  using  the  first  connected  set 

that  was  input.  Gygax  -  July  1985 

*********************************************************** xxxx* 


*** 
**  * 
**  * 
*  *  * 
*** 


X  i C*  X 

XXX 
X  X  X 
XXX 
XXX 
XXX 
XXX 


...Variable  declarations. 

INTEGERS  R1,K,IND2(2,30),IND1(30,30),I,J,IA(30) , FIRST 
INTEGERS  L I  ST ( 3  0 ) , BEG  I N , H ALT, D I  SCON , L , M, 0, TESTC , IND2R ( 2 , 3 0 ) 
INTEGERS  DATSET(30) , COUNT, SAVE(2, 30) ,  OUT 


...Initialize  the  values  of  FIRST  and  COUNT: 


FIRST  =  0 
COUNT  =  0 


...Make  vector  IA  =  list  of  all  arrays  (w/o  repeats)  in  IND2 
and  get  the  value  for  K  =  §  of  individual  arrays. 

I A ( 1 )  =  IND2 (1,1) 

I A ( 2 )  =  IND2 (2,1) 

K  =  3 

IF  ( P.l  .EQ.  1)  GOTO  60 
DO  50  I  =  1 , R1 
DO  40  J  =  1,2 
M  =  K  -  1 
DO  30  L  =  1 , M 

IF  (  I  N  D  2 ( J ,  I  )  .EQ.  I  A ( L )  )  GOTO  40 
30  CONTINUE 
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IA( K )  =  IND2 ( J , I ) 

K  =  K  +  1 
40  CONTINUE 

50  CONTINUE 

60  K  =  K  -  1 
C 

WRITE ( OUT  >  *) ' R1  ',R1 
WRITE ( OUT >  * ) ' K  1 »  K 

...  For  each  column  of  IND1  (columns  correspond  to  data  sets)  the 
entries  are  all  zero  except  for  the  row  that  corresponds  to 
the  left  array  (=  1)  and  the  right  array  (=  2). 


DO  80  I  =  1  >  R1 
DO  70  J  =  1,K 
IND1 ( J , I)  =  0 

IF  ( IND2 ( 1 > I )  .EQ.  I A ( J ) )  IND1(J,I)  =  1 
IF  ( IND2 ( 2  > I )  .EQ.  I A ( J ) )  IND1(J,I)  =  2 
70  CONTINUE 

80  CONTINUE 


...  Check  to  see 


if  all  the  arrays  are  connected. 


TESTC  =  1 
LIST(l)  =  - I A ( 1 ) 

DO  131  I  =  1  >  R1 

IF  ( I ND2 ( 1 > I )  .EQ.  -LIST(l))  IND2(1,I)  =  -IND2 (1,1) 

IF  ( IND2 (2,1)  .EQ.  -LIST(l))  IND2 ( 2 , I )  =  -IND2(2,I) 

131  CONTINUE 
BEGIN  =  1 
HALT  =  1 

140  IF  (.NOT.  (BEGIN  .LE.  HALT))  GOTO  170 
NODE  =  LIST (BEGIN) 

BEGIN  =  BEGIN  +  1 
DO  150  I  =  1,R1 

IF  ( . NOT. ( (NODE. EQ. IND2( 1, I) ) . AND. ( IND2(2, I) .GT.0) ) )  GOTO  1E0 
HALT  =  HALT  +  1 
LIST( HALT)  =  - IND2 ( 2 >  I ) 

DO  141  J  =  1,R1 

IF  ( IND2 ( 1 >  J )  .EQ.  -LIST(HALT) )  IND2(1,J)  =  - IND  2 ( 1 , J ) 

IF  ( I ND2 ( 2 , J )  .EQ.  -LIST(HALT) )  IND2(2,J)  =  -IND2(2,J) 

141  CONTINUE 

150  CONTINUE 

DC  160  I  =  1 »  R1 

IF  (  . NOT. ( ( NODE . EQ. IND2( 2, I ) )  . AND.  ( IND2( 1 ,  I )  . GT. 0)  ) )  GOTC  160 
HALT  =  HALT  +  1 
L I  ST ( HALT)  =  -  I ND2 ( 1 »  I ) 

DO  151  J  =  1 >  R1 

IF  ( IND2 ( 1 >  J )  .EQ.  -L IST( HALT) )  IND2(1,J)  =  - IND  2 (  1 , J ) 

IF  ( I ND2 ( 2  >  J )  .EQ.  -LIST(HALT) )  IND2(2,J)  =  -IND2(2,J) 

151  CONTINUE 
160  CONTINUE 

GOTO  140 
170  CONTINUE 
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C  ...Print  out  the  matched  pairs. 

DISCON  =  0 
WRITE ( OUT  >230) 

DO  200  I  =  1,R1 

IF  ( IND2 ( 1 > I )  .LT.  0)  GOTO  190 
IF  (IND2(1,I)  .EQ.  0)  GOTO  200 

IF  ( ( IND2 ( 1 > I )  .GT.  0)  .AND.  (DISCON  .EQ.  D)  GOTO  200 

FIRST  =  FIRST  +  1 

DISCON  =  1 

TESTC  =  0 

BEGIN  =  1 

HALT  =  1 

LIST(l)  =  IND2( 1 > I ) 

GOTO  200 

190  WRITE (OUT, 240)  - IND2 ( 1, I) ,-IND2(2, I) 

IF  ( (FIRST. EQ.O) .OR. ( (FIRST. EQ.l) .AND. (DISCON. EQ. 1) ) )  THEN 
COUNT  =  COUNT  +  1 
IND2R( 1, COUNT)  =  - I ND  2 ( 1 , 1 ) 

IND2R(2, COUNT)  =  -IND2(2,I) 

DATSET ( COUNT )  =  I 
END  IF 

S AVE ( 1 , I )  =  - IND2 ( 1 , I ) 

SAVE ( 2, I )  =  - I ND2 ( 2, I ) 

IND2 ( 1 , I)  =  0 
IND2 ( 2, I )  =  0 
200  CONTINUE 

IF  (DISCON  .EQ.  1)  GOTO  140 
DO  22C  I  =  1,R1 

IND2 ( 1 , I )  =  SAVE ( 1 , I ) 

I ND2 ( 2, I )  =  SAVE ( 2, I ) 

220  CONTINUE 
RETURN 

230  FORMAT( IX, 'THE  FOLLOWING  PAIRS  ARE  CONNECTED  :') 

240  FORMAT ( IX » 14 15  ) 

END 


SUBROUTINE  REDUCE  ( CROSS A, ME AN , R1 , K , I ND 1 , I A, IND2R , D ATS ET ) 

a################*####################*#################*###*# 

***  This  is  a  specialized  subroutine  that  is  used  when  K  *  * 

*  *  *  option  three  is  involked  as  a  result  of  a  failed  con-  *  *  * 
***  nectedness  test.  The  disconnected  data  sets  Must  be  *** 
***  removed  fror.i  the  variables  CROSSA  and  MEAN,  and  ether  *** 
***  program  supporting  variables  must  be  adjusted.  *  *  * 

*#*Gygax-  July  1985  *** 

###########*#########*################**#####**####***#####*#* 

...  Variable  declarations. 

IN  TE  GERM  RI ,  K ,  I NDI  ( 3  0 , 3  0  )  ,  I A  (  3  0  )  ,  I ND2P.  (  2 , 3  0  )  ,  I ,  J  ,  L ,  M ,  D  ATS  ET  ( 3  0 ) 
C 

REALM  CROSS  A  (30, 3, 3)  ,  MEAN  (3  0, 6) 

C 
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C  ...  Compute  the  new#  reduced  R1 : 

DO  10  I  =  1,30 

IF  ( I ND2R ( 1 , I )  .EQ.  0)  GOTO  20 
10  CONTINUE 
20  R1  =  I  -  1 

...  Make  a  new,  reduced  vector  IA  =  list  of  all  arrays  in 
IND2R  w/o  repeats.  Also,  compute  a  new  K. 

I A ( 1 )  =  I ND2R ( 1 , 1 ) 

I A ( 2 )  =  I ND2R ( 2 , 1 ) 

K  =  3 

IF  ( R1  .EQ.  1)  GOTO  60 
DO  50  I  =  1 ,  R1 
DO  40  J  =  1,2 
M  =  K  -  1 
DO  30  L  =  1 ,  M 

IF  ( I ND2R ( J , I )  .EQ.  IA(L))  GOTO  40 
30  CONTINUE 

I A ( K )  =  I ND2R ( J , I ) 

K  =  K  +  1 
40  CONTINUE 

50  CONTINUE 
60  K  =  K  -  1 

...  Remake  the  reduced  matrix  IND1  -  for  each  column  in  IND1 
(corresponding  to  a  data  set)  the  entries  are  zero  except 
for  the  enties  corresponding  to  the  left  array  (=  1)  and 
right  array  (=  2). 

DO  80  I  =  1 , R1 
DO  70  J  =  1 , K 
I ND 1 ( J ,  I)  =  0 

IF  ( I ND2R ( 1 » I )  .EQ.  I A ( J  )  )  IND1(J,I)  =  1 
IF  ( IND2R ( 2,1)  .EQ.  IA(J))  IND1(J,I)  =  2 
70  CONTINUE 

80  CONTINUE 

...  Reduce  the  arrays  CROSSA  and  MEAN  to  account  for  the 
removed  data  sets. 

DO  120  I  =  1 , R1 
DO  90  J  =  1,6 

MEAN ( I , J  )  =  MEAN ( DATSET ( I ) , J  ) 

SO  CONTINUE 

DO  110  J  =  1,3 
DO  100  L  =  1,3 

CROSSAC I , J , L )  =  CROSSA(DATSET(I),J,L) 

100  CONTINUE 

110  CONTINUE 

120  CONTINUE 
RETURN 
END 
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SUBROUTINE  POOL ( K , R1 , IND 1 , CROS S A, ME AN , N S 1 , IND , DE V , XB , R, N S ) 


a#*####################*#*###**############*#########*##*****#* 
***  Pools  the  means  and  the  cross  product  deviations  *** 
***  so  that  each  cross  covariance  corresponds  to  a  unique  *** 

***  sensor  array  pair.  *** 
*****(*#********#####*#####**##**####  a########*######*#**  ******* 


INTEGERM  K,R1,IND1(30,30>,IND<30,30),R,NS1(30),NS(30>,IC,IS 
REAL*8  CR0SSA(30,3 ,3 ) ,  MEAN(30,6),  DEV (30,3,3),  XB(30,6), 

+  TX(6),  DD(3,3),RNS,  DNS1 


...  Outputs  replacement  variables  IND,  DEV,  XB,  R>  and  NS 
for  use  in  MULTAR  and  INTCEP.  That  Is: 

IND  replaces  I  ND  1 ,  DEV  is  the  pooled  version  of  CR0SSA> 

XD  Is  the  pooled  version  of  MEAN,  R  replaces  Rl, 
and  NS  repl aces  NS1 . 

...  Identify  data  subsets  in  IND1  which  are  in  the  wrong  orcer 
for  use  In  the  algorithms  and  transpose  them. 

DO  20  I R= 1 , Rl 
DO  10  1=1, K 

IF  ( I ND 1 ( I , I R )  .EQ.  0)  THEN 
GOTO  5 

ELSE  IF  ( I ND 1 ( I , I R )  .EQ.  1)  THEN 
I  K=  I 

ELSE  IF  ( IND1 ( I , IR)  .EQ.  2)  THEN 
J  K=  I 

END  IF 

5  CONTINUE 
10  CONTINUE 

IF  (IK  .LT.  JK)  GOTO  20 
DO  12  1=1,3 
IP3= 1+3 

TX ( I ) =ME AN ( I R, IP3 ) 

TX ( IP3 ) =MEAN ( IR, I ) 

DO  12  J  =  1 , 3 

12  DD(  I, J )  =  CR0SSA( IR, J , I ) 

DO  15  1=1,3 
I P3  =  I +3 

MEAN ( IR, I)=TX( I) 

M E  A  N ( IR, IP3 ) =TX ( IP3) 

DO  15  J  =  1 , 3 

15  CR0SSA( IR, I, J )=DD( I, J ) 

20  CONTINUE 

...  The  transpositions  are  completed. 

...  Locate  the  subsets  to  be  pocled  and  do  the  pooling. 

Initialize  the  variables. 

IC  =  1 

C  ...  Zero  the  IND  array. 

DO  25  IT-1, K 
DO  25  I  R= 1 »  Rl 
I ND ( IT, I R ) =  0 
25  CONTINUE 
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C  ...  Start  the  algorithm. 

KM1=K- 1 
DO  40  1=1, KM1 
IP1  =1+1 
DO  40  J  =  IP  1 , K 
IS  =  0 

C  ...Zerothearrayslnitlally. 

NS(IC)  =  0 
DO  28  IT= 1 , 3 
ITP3  =  IT+3 
TX ( IT) =0 . 0D+0 
TX ( ITP3 ) =0 . 0D+0 
DO  28  J T= 1 , 3 

28  DEV ( IC, IT ,JT)=0.0D+0 

...  Initialization  Is  completed  for  this  I,J  pair. 

DO  35  I R= 1 , R1 

IF  ( I ND 1 ( I , I R)  . EQ.  0  .OR.  IND1(J,IR)  .EQ.  0)  GOTO  35 
DNS  1  =  DBLE(NSKIR)  ) 

C  ...  Set  a  flag  to  show  a  viable  I»J  pair: 

I  S=  1 

C  ...  Accumulate  weighted  sum  of  means: 

DO  32  IT= 1 , 6 

TX ( IT )  =  TX ( IT )  +  DNS1  *  MEAN ( IR, IT) 

32  CONTINUE 

C  ...  Accumulate  sums  of  crossproducts. 

DO  33  IT= 1 , 3 
DO  33  J T= 1 , 3 

D  E  V { IC, IT, JT)  =  DEV ( IC, IT, JT)  +  CROS SA ( I R, IT , J T ) 

33  CONTINUE 

C  ...  Accumulate  sample  sizes: 

NS(IC)  =  NS(IC)  +  NS  1 ( I R ) 

35  CONTINUE 

IF  (IS  .EQ.  0)  GOTO  40 

C  ...  Convert  pooled  crossproducts  into  cross  covariances. 

RNS  =  DBLE(NS( IC) ) 

DO  36  IT-1,3 
DO  36  J T= 1 , 3 

36  DEV(IC,IT,JT)=DEV(IC,IT,JT)/RNS 

C  ...  Convert  weighted  sums  into  pooled  means: 

DO  38  IT=1 , 6 

38  XB ( IC , IT )  =  TX ( IT )  /  RNS 

C  ...  Set  the  values  in  the  IND  matrix  and  update  the  counter 

I ND ( I , I C  )  =  1 
IND ( J , IC )  =  2 
IC  =  IC  +  1 
40  CONTINUE 

C  ...  When  finished,  the  counter  needs  correction. 

R= IC  -  1 

RETURN 

END 
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SUBROUTINE  I NV I ND ( K , R, I ND , FF ) 


#########################*##*#######*###############*############ 

#  #  * 
### 
#  #  # 
*  *  # 
#  fc  # 
*#* 


the  crossover  pairs. 


***  Computes  the  matrix  FF  from  IND 
***  FF  is  used  In  MULTAR  and  INTCEP 
***  IR  indexes  the  columns  of  IND,  1. 

***  I  and  J  index  the  sensor  arrays. 

***  K  is  the  number  of  arrays  in  the  problem 

***  R  is  the  number  of  (array  pair)  data  sets  in  the  problem. 
#####################*####*##*###**############################## 


INTEGERM  R,  K 

INTEGERM  IND(30,30),  FF(30,30) 

...  Initialize  F: 

DO  20  1=1, K 
DO  20  J  =  1 , K 

...  Change  the  value  of  FF  to  the  crossover  set  index  for 
those  array  pairs,  I<J,  that  are  in  the  problem. 

20  FF( I, J )  =  0 

KM1  =  K  -  1 

DO  40  1=1, KM1 
IP1  =1+1 
DO  40  J  =  IP  1 ,  K 
DO  40  IR  =  1 ,  R 

I F ( I ND ( I , I R )  .EQ.  1  .AND.  IND(J,IR)  .EQ.  2)  FF ( I , J  )  =  IR 
40  CONTINUE 

RETURN 

END 


SUBROUTINE  MULTAR ( EP , FF , DEV ,  K , R, B ) 

a*****#***#*#********#*#*##*####*#################*##*######***## 
***  This  is  an  iterative  program  to  compute  the  K  orthonorn.e  1  *** 
***  matrices  in  B.  They  must  simultaneously  optimize  the  sum  *  *  * 
***  of  traces  objective  function.  **« 
#**#*#***#***********####*#######*###*#######*#####**#####*#«**#* 


*  *  K  is  the  number  of  arrays. 

**  R  is  the  number  of  crossover  sets. 

**  EP  is  the  epsilon  for  stopping  the  iterations. 
**  DEV  is  the  set  of  pooled  crosscovariances. 

**  Calls  BMAX ,  MOE  and  MULT3 . 


...  Declarations 
INTEGERM  R,  FF(30,30),  K 

REALM  B ( 30 , 3 , 3 ) ,  E(30,3,3),  F(30,3,3),  DEV(30,3,3) 

RE  AL  *8  EP,  REP,  W  ,W0,  BB  (  3 , 3  )  ,  DD(3,3),  T(3,3),  GOO, 3, 3) 
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...  Initialize  B  as  K  Identity  matrices. 

DO  2  KK= 1 >  K 
DO  2  I  =  1,3 
DO  2  J  =  1 , 3 
B(KK,I,J>  =  0 . DO 
IF  (I  .EQ.  J)  B(KK,I,J>  =  1 . DO 
2  CONTINUE 

...  Initialize  the  temporary  arrays. 

5  DO  10  KK= 1 >  K 

DO  10  I  -1,3 
DO  10  J  =  1,3 
F(KK, I, J)-0.D0 
E(KK, I, J )=0 .DO 
0  CONTINUE 

...  Compute  WO,  the  Initial  value  of  the  objective  function. 

CALL  MOE(K,FF,DEV,B,WO) 

...  Prepare  for  the  linear  transformation  of  those  members  of  E 
designated  as  LEFT  with  the  cross  covariances. 

DO  30  KK  =  2»  K 
KKM1=KK- 1 
DO  30  JI«1,KKM1 
IR=FF( JI,KK) 

IF  (IR  .EQ.  0)  GOTO  25 

DO  15  IB  =  1,3 

DO  15  JB  =  1,3 

BB( IB, JB)  =  B( J I, IB, JB) 

DD( IB, JB)  =  DEV(IR,IB,JB) 

15  CONTINUE 

...  Perform  matrix  multiplication  and  accumulate. 

CALL  MULT3 (BB,DD,T) 

DO  20  I  =  1,3 
DO  20  J  =  1,3 

20  E( KK» I »  J )  =  E ( KK , I , J  )  +  T ( I , J ) 

25  CONTINUE 

30  CONTINUE 

...  Prepare  for  the  linear  transformations  cf  those  members  of  E 
designated  as  RIGHT  with  the  cross  covariances. 

KM1  =  K-l 
DO  40  KK-1»KM1 
KKP1  =  KK  +  1 
DO  40  J  J  =  KKP 1 , K 
IR=FF ( KK , J  J ) 

IF  (IR  .EQ.  0)  GOTO  38 
DO  32  IB  =  1,3 
DO  32  JB  =  1,3 
BB ( IB, JB)  =  B ( J  J , IB, JB) 

DD  (  IB ,  J  B  )  =  DE  V  (  IP.,  JB,  IB) 

32  CONTINUE 

C 
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C  ...  Perform  matrix  multiplication  and  accumulate. 

CALL  MULT3 (BB,DD,T) 

DO  35  I  =1,3 
DO  35  J  =  1,3 

35  F(KK, I, J )=F(KK, I, J )+T( I, J ) 

38  CONTINUE 

40  CONTINUE 

C 

C  ...  Gather  the  LEFT  and  RIGHT  accumulations. 

DO  50  KK= 1 ,  K 
DO  50  I  =1,3 
DO  50  J  =  1,3 

G(KK, I, J )  =  E(KK, I, J )+F(KK, I, J  ) 

50  CONTINUE 

C 

C  ...  Start  seeking  the  maximum  on  the  objective  surface. 

DO  60  KK  =  2 >  K 

DO  55  I  =  1,3 

DO  55  J  =1,3 

BBC  I, J )  =  B(KK, I, J) 

DD(  I, J  )  =  GCKK, I, J  ) 

55  CONTINUE 

C  ...  Subroutine  call  to  optimize  an  Indivlual  matrix  In  B. 

CALL  BMAX ( EP , DD , BB ) 

C  ...  Insert  updated  values  into  the  B  array. 

DO  58  1=1,3 
DO  58  J-1,3 

58  BCKK, I, J)  =  BB ( I , J ) 

60  CONTINUE 

C 

C  ...  Compute  new  value  of  the  objective  function. 

CALL  MOE(K,FF,DEV,B,W) 

...  Compare  with  previous  value  and  decide  whether  to 
terminate  or  Iterate. 

REP  =  EP  *  W0  /  1.D2 
IF  (DABSCW  -  W0)  .GT.  REP)  GOTO  5 

RETURN 
END 


SUBROUTINE  MOE (K , FF , DEV , B, W ) 

a*****#**#****#****##*#***###**#**##*##***************########* 

***  Computes  the  measure  of  effectiveness  for  MULTAR:  *** 

*  *  *  Output  is  W,  the  sum  of  traces  of  cross  covariances  *** 

*  *  *  modified  on  the  left  by  the  B  matrix  of  the  left  array,  *** 

*  *  *  and  on  the  right  by  the  transpose  of  the  B  matrix  of  *** 

***  the  right  array.  *** 

********************************************************  ******* 


INTEGERM  K,  FF(30,30) 

REAL*8  BOO, 3, 3),  DEV(30,3,3),  W 
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W=0 . DO 
KM1=K-1 

DO  20  IT  =  1 » KM1 
ITP1= IT+1 
DO  20  JT  =  ITP1 » K 
IR  =  FF(IT,JT) 

IF  (IR  .EQ.  0).  GOTO  IS 
DO  10  1=1,3 
DO  10  M= 1 > 3 
DO  10  J-1,3 

W=W+DEV(IR,I,M)*B(JT,J,M)*B(IT,J,I) 
10  CONTINUE 

15  CONTINUE 

20  CONTINUE 

C 

RETURN 

END 


SUBROUTINE  BMAX  (EP,D,B) 

a###############**########################*####***##**#*###*#* 
***  Iterative  program  that  climbs  the  surface  D*B-TRANS.  *** 


***  and  stops  when  the  change  is  less  than  EP  times  *** 
***  previous  level  divided  by  10.  Input  D  from  MU L TAR  and  *** 
***  output  B  which  is  a  3x3  othonormal  matrix.  *** 
***  Calls  the  subroutine  TWODIM.  *** 
***  --  August  1985.  *** 


a************************************************************* 


REAL*8  B ( 3 , 3 ) ,  D(3,3),  BB(3,3),  C(3,3),  BM(3,3) 

REAL*8  Q,  Q0,  EP,  R,  REP 

...  Initialize  the  value  on  the  surface  Q  for  the  special 
case  of  B  =  Identity  matrix: 

Q  =  0 . DO 
DO  2  1=1,3 
Q  =  Q  +  D( I,  I) 

2  CONTINUE 

...  Initialize  B  as  the  identity  matrix: 

DO  5  1=1,3 
DO  5  J-1,3 
B( I, J )  =  0 . DO 

IF  (I  .EQ.  J )  B( I, J )  =  1  .DO 

5  CONTINUE 

6  CONTINUE 

...  Compute  the  intermediate  values  C  =  D  *  B  -  TRANS  : 

DO  25  I  =  1,3 
DO  25  J  =  1,3 
C ( I , J )  =  0 . DO 
DO  25  L  =  1,3 

C  (  I ,  J  )  =  C  (  I ,  J  )  +  D  (  I ,  L  )  *  B  (  J  ,  L  ) 

25  CONTINUE 
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...  Subroutine  call  to  climb  higher  on  the  surface  of  Q: 

CALL  TWOD IM ( C , BB ) 

DO  8  I  =  1,3 
DO  8  J  =  1,3 
BM(I,J)  =  B(I,J) 

CONTINUE 

...  Replace  B  with  the  matrix  product.  This  updates  the  B  matrix. 
CALL  MULT3(BB,BM,B) 

...  Record  previous  level  on  the  surface: 

QO  =  Q 


10 


...  Compute  new  value  on  the  surface: 

Q  =  0 . DO 

DO  10  I  =  1,3 

DO  10  J  =  1,3 

Q  =  Q  +  ( D ( I , J )  *  B(I, J) ) 


...  Prepare  the  stopping  rule: 

R  =  Q  -  QO 

REP  =  EP  *  QO  /  10. 

...  Compares  the  relative  gain  with  EP;  stops  if  too  small: 
IF  (R.GT.REP)  GOTO  6 


RETURN 

END 


SUBROUTINE  TWODIM  (D,B) 

****************************************************  ******* 
***  FORTRAN  subroutine  to  compute  new  roll*  pitch*  and  *** 
yaw  matrices  ( B1 , B2 »  B3  )  and  combine  into  a  single 
orthonormal  transformation.  Receives  D  from  BKAX, 
outputs  B,  calls  MULT3 . 

—  10  June  1985. 


*  *  * 
*  *  * 
*  *  * 
*  *  * 


*  *  * 
*  *  * 
*  *  *■ 
*  *  * 


*********************************************************** 

...  Dimension  variables  and  declare  then*  to  be  double  precision 
REAL*8  6(3,3),  D(3,3),  T ( 3 , 3 ) ,  E(3,3) 

REAL*8  F ( 3 , 3 ) ,  Bl(3,3),  B2(3,3),  B3(3,3) 

RE AL*8  S,  C,  DEN0M,  SS 


...  Initialize  the  three  matrices. 

DO  5  1=1,3 

DO  5  J  =  1 , 3 

Bl( I , J ) =  0  .  DO 

B2( I >  J )  =  0 . DO 

B3 ( I , J ) =0 . DO 

CONTINUE 
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C  ...  Develop  B1  (roll  matrix)  from  Input  D: 

S  =  D(3,2)  -  D(2,3) 

C  =  D ( 2 , 2 )  +  D (3  >3  ) 

SS  =  S*S  +  C*C 
DENOM  =  DSQRT(SS) 

S  =  S  /  DENOM 
C  =  C  /  DENOM 


...  Finalize  the  B1  matrix: 

Bl(2,2)  =  C 
B1 ( 2, 3 )  =  S 
Bl(3,2)  =  -S 
Bl(3,3)  =  C 
B1 ( 1 / 1 )  =  1 . DO 

...  Update  the  Input  matrix  to  adjust  for  roll: 

. . .  E  =  D*B1 
CALL  MULT3 (D,B1,E) 


...  Develop  B2  (pitch  matrix)  from  E: 

S  =  E(3,l)  -  E(l, 3) 

C  =  E ( 3 , 3 )  +  E(lil) 

SS  =  S*S  +  C*C 
DENOM  =  DSQRT(SS) 

S  =  S  /  DENOM 
C  =  C  /  DENOM 

...  Finalize  the  B2  matrix: 

B2 ( 1 , 1 )  =  C 
B2 ( 1 , 3  )  =  S 
B2 ( 2 , 2 )  =  1 . DO 
B2 ( 3 , 1 )  =  -S 
B2 ( 3 , 3  )  =  C 

...  Additional  update  of  the  input  matrix  to  adjust  for  pitch 

...  .  F  =  E*B2 

CALL  MULT3 ( E  > B2>  F ) 

...  Develop  B3  (yaw  matrix)  from  F: 

S  =  F(2,l)  -  F ( 1 , 2 ) 

C  =  F  (  1 ,  1 )  +  F  (  2 , 2  ) 

SS  =  S*S  +  C*C 
DENOM  =  DSQRT(SS) 

S  =  S  /  DENOM 
C  =  C  /  DENOM 


...  Finalize  the  B3  matrix: 
B3  ( 1 , 1 )  =  C 
B3  ( 1  >  2  )  =  S 
B3 ( 2 , 1 )  =  -S 
B3 ( 2 , 2  )  =  C 
B3 ( 3 , 3  )  =  1 .  DO 
C 
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...  Final  triple  multiplication  of  matrices  coupled 
with  transposition: 

B_transpose  =  B1  *  B2  *  B3 
DO  30  1=1/3 
DO  30  J  =  1 , 3 
B(J,I)  =  0 . DO 
DO  30  IK=1,3 
DO  30  KJ=1,3 

B(J»I)=B(J,I)+B1(I,IK)*B2(IK»KJ)*B3(KJ,J) 

30  CONTINUE 

RETURN 
END 


SUBROUTINE  MULT3(A,B,C) 

***************************************-******************** 

***  Multiplies  the  3  by  3  matrices  A  and  B  to  get  C  *** 
*********************************************************** 


REAL*8  A  ( 3 , 3  )  ,  B(3 ,3  ) ,  C(3 ,3  ) 

C 

DO  10  1=1,3 
DO  10  J  =  1 , 3 
C(I,J)=  0 . DO 
DO  10  K= 1 , 3 

C  (  I ,  J  )  =  C(I,J)  +  A(I,K)*B(K,J) 
10  CONTINUE 
RETURN 
END 


C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 


SUBROUTINE  I NTCEP ( FF , NS , B , XB , K , R, A ) 

**************************************************************** 

*  *  *  Sets  up  and  solves  the  system  of  linear  equations  for  *  *  * 

***  each  component  of  the  set  of  K  intercept  vectors.  *** 

***  Requires  the  output  of  POOL  and  MULTAR.  *** 

***  --  June  1985.  *** 

*******  *******************************************************  K  * 

**  K  is  the  number  of  sensor  arrays  in  the  problem. 

**  R  is  the  (pooled)  number  of  crossover  data  sets. 

*  *  NS  is  the  vector  of  sample  sizes  for  the  crossover  sets. 

**  FF  is  the  upper  triangular  K  by  K  matrix  whose  values  are 

*  *  either  zero  or  the  index  of  the  data  set  for  the  array  pair 
**  identified  by  the  subscripts. 

**  XB  is  the  R  by  6  matrix  of  means  for  each  of  the  two  arrays 
**  identified  with  each  of  the  R  data  sets. 

**  B  is  the  K  by  3  by  3  collection  of  orthonormal  orientation 
**  correction  matrices  returned  from  MULTAR. 

**  The  output  A  is  the  K  by  3  matrix  of  intercept  values 
**  estimated  for  each  of  the  K  arrays  in  the  problem. 
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Calls  MMATMUL  which  performs  linear  transformations. 
Calls  SYSLIN  which  solves  systems  of  linear  equations. 

Dec! a  rat  Ions. 


•  •  • 


INTEGERS  R,  FF(30,30),  NS(30),  K,  I,  IR,  IP1 

REAL*8  B(30,3»3),XB(30,6)»M(30,30), 

1  LHS (3 0,3 ) , XL (30,3 ) ,XLL( 30,3) ,XXL( 30,3) ,XR( 30,3 ) ,XRR(30,3 ) , 

2  XXR(30,3),  E  (3  0 , 3  ) ,  F  (3  0 »  3  ) »  A(3 0 , 3  ) »AA(30»31) »DNS 

...  Begin  development  of  the  coefficient  matrix  M,  off  diagonal 
terms.  Also  prepare  the  Inputs  for  the  summands  on  the 
other  side  of  the  equation.  Affixes  L  and  R  are  used  to 
suggest  the  left  and  right  portions  of  the  expressions. 

...  The  values  In  the  coefficient  matrix  are  zero  whenever  the 
array  pair  Is  not  Identified  with  a  crossover  data  set,  i.e. 
when  FF(1,j)=0  and  1  is  less  than  j.  Otherwise  its  value  is 
the  negative  of  the  sample  size  for  the  data  set. 


K  M 1  =  K  - 1 
DO  10  I-l.KMl 
IP1  =1+1 
DO  10  J  =  IP1 »  K 
IR-FF(  I. J  ) 

IF  (IR  .EQ.  0)  THEN 
MCI, J  )  =  0 . DO 
M  ( J  ,  I )  =  0 . DO 
GOTO  10 

END  IF 

M  ( I » J  )  =  -1 . DO  *  DBLE(NS( IR) ) 
M  (  J  ,  I )  =  M  (  I ,  J  ) 

DO  5  11-1,3 


...The  weighted  version  requires  that  the  means  be  multiplied 
by  NS.  The  first  three  columns  of  XB  are  identified  with  the 
left  (L)  array  of  the  pair;  the  last  3  with  the  right  (P. ). 
This  prepares  inputs  for  summands  on  the  other  side  of  the 
equation. 

DNS  =  DBLE ( NS ( IR) ) 

XL(IR,II)=DNS*XB(IR,II) 

IIP3  =3+11 

5  XR( IR, II)=DNS*XB( IR, IIP3 ) 

10  CONTINUE 


. ..  Finish  development  of  the  M  matrix.  The  Qi agonal  term  in  a 
row  is  the  negative  of  the  total  of  the  other  terms  in  that 
row.  The  first  column  of  E  is  used  to  accumulate  terms. 
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12 


DO  15  1  =  1, K 
M ( I ,  I )  =  0 . DO 
E  ( 1 , 1 )  =  0 . DO 
DO  12  J  =  1 , K 

E  ( 1 , 1 )  =  E  (  1 , 1 )  +  M  (  I ,  J  ) 

M ( I , I )  =  -1 . DO  *  E (  1 , 1 ) 

15  CONTINUE 

...  The  coefficient  matrix  is  complete. 

Next  develop  the  sequence  of  partial  sum  arrays  for  use  in 
the  LHS  of  the  system  of  equations.  First  Initialize. 

Then  treat  the  left  array  partial  sums. 

DO  25  KK=2, K 
DO  22  J  =  1 , 3 
XXL ( KK , J ) =0 . DO 
XLL ( KK, J ) =0 . DO 

22  CONTINUE 
KKM1=KK-1 

DO  25  >1,KKM1 
IR  =  FF ( I , KK ) 

IF  (IR  .EQ.  0)  GOTO  25 

CALL  MMATML ( K > R, I , IR, IR,B,XL,F) 

DO  23  J  =  1 , 3 

XXL ( KK , J )  =  XXL ( KK , J  )  +  F ( I R , J ) 

23  XLL ( KK, J )  =  XL  L ( KK , J )  +  XR(IR,J) 

25  CONTINUE 

...  Next  develop  the  right  array  partial  sums. 

Again  the  initial  values  must  be  zero. 

DO  30  KK* 1 , KM1 
DO  26  1=1,3 
XXRCKK, I ) =0 . DO 
XRR(KK, I )=0.D0 

26  CONTINUE 
KKP1=KK+1 

DO  30  J  =  KKP 1 , K 
I R=  FF ( KK , J ) 

IF  ( IR  .EQ.  0)  GOTO  30 

CALL  MMATML(K,R, J, I R , I R , B , XR , F ) 

DO  27  1=1,3 

XXRCKK, I)  =  XXR ( KK ,  I  )  +  F(IR,I) 

27  XRR ( KK , I )  =  X  R  R ( K  K , I )  +  XL(IR,I) 

30  CONTINUE 

...Collect  the  above  quantities  into  the  LHS  of  the  system 
of  equations.  Omit  the  end  terms  for  now. 

DO  35  KK=2 , KM1 
DO  31  11=1,3 

31  XL ( K ,  1 1  )  =  XLL ( KK ,  1 1  )  +  XRR(KK,II) 

CALL  MMATML (K,R,KK,K,K,B,XL,F) 

DO  33  11=1,3 

3  3  LHS  (  KK , II)  =  XXL(KK,II)+XXR(KK, II )-F(K,  II) 

35  CONTINUE 

C 
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C  ...Finalize  with  the  end  correction  terms. 

C 

CALL  MMATML(K,R,1,1,1,B,XRR,F) 

DO  38  I  =  1,3 

LHS ( 1 , I )  =  XXR ( 1 , I )  -  F(1,I) 

38  CONTINUE 

CALL  MMATML ( K, R, K» K# 1,B»XLL,F) 

DO  40  1*1,3 

40  LHS ( K, I )  =  XXL ( K , I )  -  F ( 1 , 1 ) 

...  SOLVE  THE  LINEAR  SYSTEM  MA=L.  This  must  be  done  three  tin.es,  * 
once  for  each  column  of  LHS.  Since  the  matrix  M  has  rank  K-l 
and  since  the  solution  is  to  be  made  relative  to  the  first 
array,  we  trim  away  the  first  row  and  first  column  of  M  to 
get  a  non-singular  system.  For  the  same  reason  the  elements 
of  the  first  row  of  A  are  all  set  to  zero. 

DO  50  1=1,3 

...  Prepare  the  input  to  SYSLIN  for  each  column  of  LHS. 

DO  48  KI=2, K 
DO  45  K J  =  2 , K 
KIM1-KI-1 
KJM1=KJ -1 

AA(KIM1,KJM1)=M(KI,KJ ) 

45  CONTINUE 

48  AA(KIM1,K)=LHS(KI, I) 

...  Solve  the  system. 

CALL  SYSLIN(AA,30,KM1) 

A ( 1 , I )  =  0 . DO 

...  Place  the  solution  into  the  output  matrix  A. 

DO  50  KK  =  2 »  K 
KKM1=KK-1 

A(KK, I)=AA(KKM1,K) 

50  CONTINUE 


RETURN 

END 


SUBROUTINE  MULMAT( BB , EE , EF ) 

********************************************************** 
***  Subroutine  to  perform  a  linear  transformation  in  *** 
***  three-space.  Specifically,  EF  is  the  product  of  *** 

***  the  matrix  BB  and  the  vector  EE.  *  *  * 

********************************************************** 

REAL*8  BB ( 3 , 3  )  ,  EF(3>,  EE ( 3  ) 

...  Initialize  EE  at  zero: 

DO  10  I  =  1,3 
EF ( I )  =  0. DO 
10  CONTINUE 
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DO  20  I  =  1,3 
X  =  0  .  DO 

C  ...  Accumulate  the  dot  product  of  EE  and  the  I-th  row  of  BB: 

DO  30  J  =  1,3 

30  X  =  X  +  BB ( I ,  J  )  *  EE ( J ) 

EF ( I )  =  X 
20  CONTINUE 
RETURN 
END 


SUBROUTINE  SYSLIN ( A, IR, IC ) 

a#######*###*#**############################*###########******* 

***  Finds  the  solution  of  a  system  of  IC  equations  in  IC  *** 

***  unknowns.  *** 

a##**##*#*########**##########*##############*#**#**#********** 

RE AL*8  A (  1 ) 

ICP1  *  IC  +  1 
DO  107  K  =  1,IC 
INDEX1  =  (K-1)*IR  +  K 
A ( INDEX1 )  =  1 .D0/A( INDEX1  ) 

DO  102  J  =  1,ICP1 
IF(J-K)  3,102,3 
3  INDEX2  =  (J-1)*IR  +  K 

A ( I ND  EX2 )  =  A( INDEX2 ) *A ( INDEX1 ) 

102  CONTINUE 

DO  115  I  =  1, IC 
IF  (I-K)  20,115,20 

20  INDEX3  =  (K-1)*IR  +  I 
DO  114  J  =  1,ICP1 
IF(J-K)  21,114,21 

21  INDEX2  =  (J-1)*IR  +  I 
INDEX4  =  (J  -  1 ) *IR  +  K 

A ( I ND EX2 )  =  A( INDEX2 )  -  A( INDEX4 ) *A ( INDEX3 ) 

114  CONTINUE 

115  CONTINUE 

DO  107  I  =  1 , I C 
I F (  I-K )  22,107,22 

22  INDEX2  =  (K-1)*IR  +  I 

A ( I NDEX2 )  =  - 1 . DO  *  A( INDEX2) *A ( INDEX1) 

107  CONTINUE 
RETURN 
END 
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SUBROUTINE  MMATML ( K , R, L , M, N , B , E , F ) 


xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
***  Prepares  for  a  linear  transformation  by  a  selected  *** 


***  Lth  face  of  B;  calls  MULMAT  to  perform  the  *** 
***  transformation  on  the  Mth  row  of  E  and  positions  *** 
***  the  new  vector  In  the  Nth  row  of  F.  *** 
***  K  =  number  of  arrays  In  the  problem  *** 
***  R  =  number  of  rows  In  (common  to)  E  and  F  *** 
###  l  =  array  Index;  first  argument  of  B  *** 
***  M  =  Index  of  the  row  of  E  *** 
***  N  =  Index  of  the  row  of  F  *** 
***  B  =  Rank  3  family  of  rotational  matrices  *** 
***  EE  =  multiplicand  vector  *  *  * 
***  EF  =  product  vector  *** 


***  E  =  Input  array  (of  rank  2),  row  M  is  used  for  EE  *** 
###  p  =  output  array  (of  rank  2 )  ,  E  F  is  placed  In  row  N  *** 
xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 


% 


X 


INTEGERS  I,  J,  K,  L,  M,  N  ,R 

REAL*8  BB ( 3  >  3  )  ,  BOO, 3, 3),  EE(3),  EF(3),  E(30,3),  F(30,3) 


...  Initialize  the  temporary  variables: 

DO  10  I  =  1,3 
EE ( I )  =  E ( M , I ) 

DO  10  J  =  1,3 

BB ( I ,  J  )  =  B ( L ,  I ,  J  ) 

10  CONTINUE 

...  Call  routine  to  perform  linear  t r an s f o rma t i on 
CALL  MULMAT ( BB , EE, EF ) 

...  Position  the  output  as  prescribed. 

DO  20  I  =  1,3 
F ( N , I )  =  EF ( I ) 

20  CONTINUE 

RETURN 

END 


SUBROUTINE  DISPLC(K,B,A,IA,DEL,D) 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
xxx  DEL  is  the  set  of  (output)  displacements  of  the  K  arrays  *** 
***  estimated  bythe  least  square  method.  *** 
xxx  d  is  the  length  of  each  rovv  of  DEL.  **„* 
***  B  is  the  set  of  K  reorientation  matrices  (output  of  MULTAR) .  *** 
***  A  is  the  output  of  INTCEP  (i.e.  the  array  intercept  vectors).*** 
***  IA  is  the  list  of  sensor  arrays  in  the  problem.  *  *  * 
xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
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Dec! arat 1 ons 


•  •  • 


REAL*8  BOO, 3, 3),  A(30,3),  AL(30,3),  DEL(30,3),  AA(3) 

REAL#8  IDO, 3),  BB(3,3),  D(30),  ARNUM1 ,  ARNUM2,  ARNUM3 ,  T(3) 
INTEGERM  IAOO),  K,  DATE 

...  Create  an  Identity  matrix  ID: 

DO  10  1=1,3 

DO  10  J  =  1 , 3 

ID ( I ,  J  )  =  0 . DO 

IF  (  I  .NE.  J  )  GOTO  9 

I D ( I , J )  =  1 . DO 

9  CONTINUE 

10  CONTINUE 

...  Read  file  AR1.DAT  In  the  order  specified  by  IA. 

This  Is  used  to  create  AL,  the  matrix  of  assumed  locations 
of  those  sensor  arrays  that  appear  in  the  problem. 

The  Identlcatlon  vector  IA  produced  In  CONECT  Is  needed  to 
extract  the  correct  rows  from  AR1.  The  result  Is  AL(K,3). 

0PEN(2,FILE='AR1.DAT» , STATUS= ' OLD ' ) 

J  =  1 

17  CONTINUE 

READ(2»*,END=18)  IAR,  DATE,  ARNUM1,  ARNUM2 ,  ARNUM3 
GOTO  15 

18  CONTINUE 

WRITE(*,*)  *  Designated  array  number  not  found.  ' 

WRITE(*>*)  '  Operation  aborting.  ' 

STOP 

15  CONTINUE 

IF  (IAR  .EQ.  I A ( J  )  )  THEN 
AL ( J  ,  1 )  =  ARNUM1 
AL ( J , 2 )  =  ARNUM2 
AL ( J , 3 )  =  ARNUM3 
J  =  J  +  1 
REWIND  (2) 

END  I F 

I F ( J  .LE.  K)  GOTO  17 
CLOSE  ( UNIT=2 ) 

...  Compute  the  displacements. 

...  First  reduce  by  one  the  diagonal  elements  of  each  face  of  E: 
DO  30  KK= 1 , K 
DO  20  1=1,3 
DO  20  J  =  1 , 3 

BB (  I , J )  =  B ( K K , I »  J )  -  I D (  I ,  J ) 

20  CONTINUE 


...  Complete  the  displacement  computation: 
DO  22  1=1,3 
22  A A (  I  )  =  AL (KK,  I  ) 

CALL  MULMAT ( BB , AA ,  T ) 
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DO  25  1=1,3 

DEL ( KK, I ) =  A ( KK ,T ) +T ( I ) 

25  CONTINUE 
30  CONTINUE 

...  Compute  the  length  of  each  row  of  DEL. 

DO  40  KK= 1 , K 
D ( KK )  =  0 . DO 
DO  35  1=1,3 

D ( KK )  =  D ( KK )  +  ( DEL ( KK, I )  *  DEL ( KK, I ) ) 

35  CONTINUE 

40  D ( KK )  =  DSQRT ( D ( KK ) ) 

RETURN 

END 


SUBROUTINE  MAXANGC  BB , P) 

REAL*8  B ( 3 , 3 ) ,  BB(3,3),  X ( 3 ) ,  Y(3),  D,  P 

*************************************************************** 
***  Takes  the  orthonormal  matrix  BB  as  input  and  constructs  *** 
***  the  angle  of  maximal  rotation  that  any  vector  can  *** 

***  experience  under  the  transformation  BB.  *** 

***  Calls  MULT3 .  *** 

********************************************************  ******* 


...  Begin  with  the  adjustment  of  the  diagonal  elements  of  E 
in  order  to  prepare  the  eigenvector  problem. 

DO  5  1=1,3 
DO  5  J  =  1 , 3 
B ( I , J )  =  BB ( I ,  J  ) 

IF  (I  .EQ.  J)  B ( I , I )  =  B ( I , I )  -  1. 

5  CONTINUE 

...  Use  the  first  two  equations  of  the  eigenvector  system 
(with  X3  =  1)  to  solve  for  the  eigenvector. 

...  Compute  the  determinant  of  the  coefficient  matrix: 

D  =  B(1,1)*B(2,2)  -  B(1,2)*B(2,1) 

...  Check  for  zero  rotation  and  transfer  to  the  end 
if  it  occurs. 

IF  (D  .EQ.  0.0)  THEN 
D  =  1 . DO 
GOTO  30 
END  IF 

...  Set  third  component  equal  to  one  and  solve  the  linear 
system  for  the  other  two. 

X ( 3 )  =  1 . DO 

X ( 1 )  =  (B(1,2)*B(2>3)  -  B(1,3)*B(2,2))/D 
X ( 2 )  =  (B(l,3)»6(2,l)  -  B(l,l)*6(2,3))/D 

...  Normalize  the  eigenvectors. 
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10 

c 

c 

c 

*  c 

p 

D  =  DSQRT ( 1 . DO  +  X(1)*X(1)  +  X(2)*X(2)> 

DO  10  1=1,3 

X(I)  =  X(I)/D 

...  Construct  a  vector  orthogonal  to  the  eigenvector. 

First  choose  the  subscript  of  the  smallest  X. 

J  =  1 

IF  ( DABS ( X ( 1 ) )  .GT.  DABS  C  X  C  2  >  > )  J  =  2 

IF  ( DABS  C  X ( J ) )  .GT.  DABSCXC3 ) ) )  J=3 

c 

c 

15 

c 

...  Use  the  Gram-Schmldt  technique  to  convert  the  direction 
of  X(J)  to  a  direction  orthogonal  to  X(l),  X(2),  X ( 3 )  . 

DO  15  1=1,3 

IF  (I  .EQ.  J)  THEN 

Y  ( J  )  =  X(J)*(1.-  X(J)*X(J>) 

ELSE 

Y ( I )  =  (-1. )*X(I)*X(J )*X(J ) 

END  IF 

CONTINUE 

...  Normalize  the  new  vector. 

D  =  DSQRT (  Y ( 1 ) * Y ( 1 )  +  Y(2)«Y(2)  +  Y(3)*Y(3)  ) 

DO  20  1=1,3 

20 

C 

C 

r 

Y( I)  =  Y( I)/D 

...  Transform  Y  by  BB  and  compute  the  angle  between  Y  and  BB*Y 

25 

30 

CALL  MULMAT ( BB , Y , X ) 

D  =  0 . DO 

DO  25  1=1,3 

D  =  D  +  X( I) *Y( I) 

CONTINUE 

P  =  DACOS(D) 

RETURN 

END 

C 

C 

C 

C 

C 

C 

C 

SUBROUTINE  ANGLE S ( K , B , P ) 

****######*#############*#####**###*#*####*##*#####*#*##* 

***  Computes  the  three  Euler  angles  for  each  of  the  *  *  * 

***  K  matrices  in  B.  *  *  * 

***************************************  **********  ******** 

RE AL *8  6(30, 3, 3  ) ,P(30,3) 

DO  20  KK* 1 ,  K 

9 

P ( KK , 2 ) =D AS  IN  ( B ( KK , 3 , 1 )  ) 

P ( KK , 1 ) =DAS IN  ( B ( KK , 3 , 2 ) /DCCS  (P(KK,2))) 

PC  KK  ,3  )  =DASIN  (  B  (  KK  ,  2, 1  )  /DC0S  (P(K.K,2))) 

20 

« 

CONTINUE 

RETURN 

END 
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